Введение:
В этом уроке мы дополним робота «Дройдика» модулем Bluetooth и создадим пульт дистанционного управления на базе аналогичного Bluetooth модуля. Управление направлением и скоростью движения робота будет осуществляться с помощью джойстика, а при обнаружении препятствий «Дройдик» откажется идти прямо, но согласится пойти назад.
Bluetooth модуль пульта будет выполнять роль мастера, а Bluetooth модуль робота - роль ведомого. Сопряжение мастера и ведомого достаточно выполнить только один раз. В дальнейшем, при подаче питания робота и питания пульта, устройства будут соединяться самостоятельно.
Скорость и направление движения робота будет зависеть от степени и направления отклонения джойстика. Робот сможет выполнять такие команды как движение вперёд или назад, с заворотом или без, разворот на месте влево или вправо и устанавливать все суставы в центральные положения. При разрыве связи между bluetooth модулями, робот перестанет идти, а все его сервоприводы ослабнут.
Подробно об управлении роботом и сопряжении Bluetooth модулей рассказано ниже, в разделе «Управление».
Видео:
Нам понадобится:
Робот "Дройдик":
- Робот-платформа «Дройдик» x1 шт.
- Arduino Uno x1 шт.
- Tream-Power Shield x1 шт.
- Trema-модуль Bluetooth HC-05 x1 шт.
- Trema-модуль кнопка x1 шт.
- Аккумулятор Ni-MH размером AA x5 шт.
(либо два аккумулятора Li-ion размером 18650 и батарейный отсек для них).
Пульт:
- Arduino Uno x1 шт.
- Trema-Set Shield x1 шт.
- Trema-модуль Bluetooth HC-05 x1 шт.
- Trema-модуль джойстик x1 шт.
- аккумулятор Ni-MH (крона) и переходник под DC-jack x1 шт.
(либо иной источник питания для Arduino Uno).
Для реализации проекта нам необходимо установить библиотеки:
- iarduino_Bluetooth_HC05 - для работы с Trema Bluetooth модулем HC-05.
- iarduino_HC_SR04_int - для работы с ультразвуковым датчиком расстояния HC-SR04+.
- Библиотеки SoftwareSerial и Servo входят в базовый набор Arduino IDE и не требуют установки.
О том как устанавливать библиотеки, Вы можете ознакомиться на странице Wiki - Установка библиотек в Arduino IDE.
Схема подключения пульта дистанционного управления:
- Установите Trema-Set Shield на Arduino Uno.
- Установите Trema-модуль джойстик на 4 посадочную площадку Trema-Set Shield.
- Установите Trema-модуль Bluetooth HC-05 на 3 посадочную площадку Trema-Set Shield.
- При желании все установленные на Trema-Set Shield модули можно закрепить, используя нейлоновые винты и стойки.
| Модули | Trema Set Shield |
|---|---|
| Trema-bluetooth (колодка RX-G-V-K-TX) | 3 площадка (выводы 8-G-V-3-9) |
| Trema-джойстик (колодка Y-G-V-K-X) | 4 площадка (выводы A1-G-V-2-A2) |
Благодаря применению Trema-Set Shield все модули устанавливаются без проводов, а наличие на каждой площадке всего двух колодок с разным количеством выводов, гарантирует правильность установки модулей.
Схема подключения робота «Дройдика»:
Соберите механику, подключите Tream-Power Shield, сервоприводы, датчик расстояния и откалибруйте робота, как это описано в уроке 38 Сборка «Дройдика». Далее на боковые панели установите Bluetooth HC-05 и кнопку, первый модуль подключается к шине UART (в примере используется аппаратная шина UART), а второй к любому выводу (в примере используется вывод D9).
| Датчик расстояния | Trema Power Shield | |
|---|---|---|
| Датчик HC-SR04+ |
вывод Echo | вывод 2 на белой колодке |
| вывод Trig | вывод 3 на белой колодке | |
| вывод Vcc | любой вывод на красной колодке | |
| вывод Gnd | любой вывод на чёрной колодке | |
Вы можете изменить вывод 3 используемый для подключения вывода Trig на любой другой, указав его при определении константы pinTrig. Но вывод Echo датчика HC-SR04+ можно подключать только к выводам с аппаратным прерыванием (2 или 3) так как в скетче используется библиотека iarduino_HC_SR04_int (если использовать библиотеку iarduino_HC_SR04, то и вывод Echo можно будет подключать к любым выводам).
| Сервоприводы | Trema Power Shield | |
|---|---|---|
| Верхние суставы | Левая нога «Дройдика» | вывод 4 на белой колодке |
| Правая нога «Дройдика» | вывод 5 на белой колодке | |
| Нижние суставы | Левая нога «Дройдика» | вывод 6 на белой колодке |
| Правая нога «Дройдика» | вывод 7 на белой колодке | |
Вы можете изменить выводы 4-7 для подключения сервоприводов на любые другие, указав их в скетче при определении констант pinLeftTop, pinRightTop, pinLeftBot, pinRightBot.
Трехпроводные шлейфы сервоприводов устанавливаются следующим образом:
- Оранжевый провод подключается к выводу на белой колодке.
- Красный провод подключается к выводу на красной колодке.
- Коричневый провод подключается к выводу на чёрной колодке.
| Bluetooth | Trema Power Shield | |
|---|---|---|
| Bluetooth HC-05 | вывод RX | вывод TX на колодке Serial |
| вывод TX | вывод RX на колодке Serial | |
| вывод K (Key) | вывод D10 на белой колодке | |
| вывод V (Vcc) | любой вывод на красной колодке | |
| вывод G (GND) | любой вывод на чёрной колодке | |
Вы можете изменить вывод D10 для подключения Bluetooth на любой другой, указав его в скетче при определении константы pinBTKey.
Выводы RX и ТХ модуля подключаются проводами к выводам TX и RX колодки с надписью Serial. Трёхпроводной шлейф подключённый к выводам K, V, G, устанавливается следующим образом:
- Вывод K (Key) подключается к выводу на белой колодке.
- Вывод V (Vcc) подключается к выводу на красной колодке.
- Вывод G (GND) подключается к выводу на чёрной колодке.
| Кнопка | Trema Power Shield | |
|---|---|---|
| Trema-кнопка | вывод S (Signal) | вывод D9 на белой колодке |
| вывод V (Vcc) | любой вывод на красной колодке | |
| вывод G (GND) | любой вывод на чёрной колодке | |
Вы можете изменить вывод D9 для подключения кнопки на любой другой, указав его в скетче при определении константы pinKey.
Трёхпроводной шлейф подключённый к выводам S, V, G, устанавливается следующим образом:
- Вывод S (Signal) подключается к выводу на белой колодке.
- Вывод V (Vcc) подключается к выводу на красной колодке.
- Вывод G (GND) подключается к выводу на чёрной колодке.
Представленная ниже схема совпадает со схемой из инструкции по сборке «Дройдика», но к ней добавились два модуля: bluetooth (подключается к выводам D10, TX и RX) и кнопка (подключается к выводу D9).
Элементы схемы: «Э1»...«Э4» - сервоприводы, «Э5» - датчик расстояния, «Э6» - Trema blurtooth модуль, «Э7» Trema кнопка.
Код программы для пульта дистанционного управления:
// Подключаем библиотеки: //
#include <SoftwareSerial.h> // Подключаем библиотеку SoftwareSerial для общения с модулем по программной шине UART.
#include <iarduino_Bluetooth_HC05.h> // Подключаем библиотеку iarduino_Bluetooth_HC05 для работы с Trema Bluetooth модулем HC-05 (ссылка на библиотеку: https://iarduino.ru/file/301.html).
// Объявляем объекты для работы с библиотеками: //
SoftwareSerial objSerial(9, 8); // Создаём объект objSerial указывая выводы (RX, TX) Arduino UNO, можно указывать любые выводы. Вывод RX Arduino подключается к выводу TX модуля, вывод TX Arduino подключается к выводу RX модуля.
iarduino_Bluetooth_HC05 objHC05(3); // Создаём объект objHC05 указывая любой вывод Arduino, который подключается к выводу «K» Bluetooth модуля.
// Определяем константы хранящие номера выводов Arduino: //
const uint8_t pinX = A2; // Определяем константу pinX указывая номер вывода Arduino к которому подключён вывод X джойстика (аналоговый).
const uint8_t pinY = A1; // Определяем константу pinY указывая номер вывода Arduino к которому подключён вывод Y джойстика (аналоговый).
const uint8_t pinK = 2; // Определяем константу pinK указывая номер вывода Arduino к которому подключён вывод K джойстика (цифровой).
// Определяем константы задающие люфт для центра джойстика: //
const uint16_t gapX = 30; // Изменение положения джойстика на +-gapX от центрального значения по оси X, будет восприниматься как 0
const uint16_t gapY = 30; // Изменение положения джойстика на +-gapY от центрального значения по оси Y, будет восприниматься как 0
// Объявляем переменные и массивы: //
uint16_t varX, cenX; // Объявляем переменную varX для хранения текущего положения джойстика по оси X и переменную cenX для хранения центрального положения джойстика по оси X.
uint16_t varY, cenY; // Объявляем переменную varY для хранения текущего положения джойстика по оси Y и переменную cenY для хранения центрального положения джойстика по оси Y.
bool varK; // Объявляем переменную varK для хранения состояния кнопки джойстика.
int8_t arrData[3]; // Объявляем массив arrData значения которого будем передавать по bluetooth, можно создавать массивы или переменные любых типов в т.ч. и char
//
void setup(){ //
// Устанавливаем режимы работы объявленных выводов: //
pinMode(pinX, INPUT); // Устанавливаем режим работы вывода pinX как вход.
pinMode(pinY, INPUT); // Устанавливаем режим работы вывода pinY как вход.
pinMode(pinK, INPUT); // Устанавливаем режим работы вывода pinK как вход.
// Считываем показания с кнопки джойстика: // Для входа в режим поиска и сопряжения с ведомым bluetooth модулем
varK=digitalRead(pinK); // Читаем логический уровень со входа pinK в переменную varK.
// Инициируем работу с bluetooth модулем: //
while(!objHC05.begin(objSerial)){;} // Инициируем работу с bluetooth модулем, указывая имя объекта или класса для управления шиной UART. При провале инициализации функция begin() вернёт false и тогда оператор while запустит её вновь.
// Определяем центральные положения джойстика по осям X, Y: //
cenX=analogRead(pinX); // Читаем уровень сигнала со входа pinX в переменную cenX, этот уровень будет считаться центральным положением джойстика.
cenY=analogRead(pinY); // Читаем уровень сигнала со входа pinY в переменную cenY, этот уровень будет считаться центральным положением джойстика.
// Выполняем поиск и сопряжение с ведомым bluetooth модулем: // Если сопряжение не выполнить (не нажать на джойстик при старте), то bluetooth модуль создаст соединение на основе своих старых настроек.
if(varK){while(!objHC05.createMaster("Droidik","1234")){;}} // Если кнопка джойстика нажата при старте, то устанавливаем bluetooth модулю роль мастера и пытаемся подключиться к ведомому с именем "Droidik" и PIN-кодом 1234. Если ведомый не доступен, то функция createMaster() вернёт false и тогда оператор while запустит её вновь.
while(!objHC05.checkConnect()){delay(1000);} // Проверка подключения к ведомому Bluetooth устройству. Если ведомый недоступен, то функция checkConnect() вернёт false и тогда оператор while запустит её снова через паузу в 1000 мс.
} //
//
void loop(){ //
// Считываем показания с джойстика: //
varX=analogRead(pinX); // Читаем уровень сигнала со входа pinX в переменную varX
varY=analogRead(pinY); // Читаем уровень сигнала со входа pinY в переменную varY
varK=digitalRead(pinK); // Читаем логический уровень со входа pinK в переменную varK
// Преобразуем считанные показания и добавляем люфт: //
if(varX<cenX-gapX){varX=map(varX,cenX-gapX, 0,0,-100);}else // Преобразуем значения varX от диапазона cenX-люфт...0 к диапазону 0...-100
if(varX>cenX+gapX){varX=map(varX,cenX+gapX,1023,0, 100);}else // Преобразуем значения varX от диапазона cenX+люфт...1023 к диапазону 0...+100
{varX=0;} // Оставшиеся значения cenX+-люфт преобразуем в 0
if(varY<cenY-gapY){varY=map(varY,cenY-gapY, 0,0,-100);}else // Преобразуем значения varY от диапазона cenY-люфт...0 к диапазону 0...-100
if(varY>cenY+gapY){varY=map(varY,cenY+gapY,1023,0, 100);}else // Преобразуем значения varY от диапазона cenY+люфт...1023 к диапазону 0...+100
{varY=0;} // Оставшиеся значения cenY+-люфт преобразуем в 0
// Сохраняем полученные значения в массив и передаём его: //
arrData[0]=int8_t(varX); // Положение джойстика по оси X в диапазоне от -100 до +100 с добавлением люфта по середине.
arrData[1]=int8_t(varY); // Положение джойстика по оси Y в диапазоне от -100 до +100 с добавлением люфта по середине.
arrData[2]=int8_t(varK); // Состояние кнопки джойстика 0 или 1.
objHC05.send(arrData); // Отправляем массив arrData на ведомое bluetooth устройство.
delay(50); // Примечания:
} // Функция send работает только при передаче данных на внешний Trema Bluetooth модуль использующий данную библиотеку!
// Если не ввести такое понятие как люфт для осей X (gapX) и Y (gapY), тогда будет трудно найти положение джойстика при котором квадропед будет стоять неподвижно.
В данном коде показания джойстика постоянно сохраняются в массив arrData после чего он отправляется по радиоканалу через bluetooth модуль. Скетч калибрует джойстик при старте, считывая показания для осей X и Y, которые до отключения питания считаются центральными. Показания осей X и Y отправляются в пределах значений от -100 до +100, а состояние кнопки джойстика отправляется как число 0 или 1.
Если подать питание с нажатой кнопкой джойстика, то в коде setup выполнится код вызова функции createMaster, которая установит bluetooth модулю роль мастера, инициирует поиск ведомого с именем "Droidik" и PIN-кодом "1234", и если такое ведомое устройство будет доступно, то произойдёт сопряжение и соединение с этим ведомым bluetooth модулем (именно такое имя и PIN будут присвоены bluetooth модулю на роботе). Если не нажимать на джойстик при подаче питания, то функция createMaster будет пропущена, а bluetooth модуль будет принимать попытки создать соединение на основе своих последних настроек. Таким образом сопряжение с bluetooth модулем робота достаточно выполнить всего один раз.
Функции begin(), createMaster() и checkConnect() объекта objHC05 возвращают true или false, и вызываются как условие оператора while(), то есть инициализация, назначение роли и проверка соединения bluetooth модулей выполняются до тех пор пока не будет получен положительный результат. Эти функции можно вызывать однократно только в том случае если Вы уверены что второй bluetooth модуль (модуль робота) точно включён, ему назначена роль ведомого, он готов к соединению и находится в радиусе действия связи. Иначе функция вернёт false, а код продолжит выполняться не отреагировав на ошибку.
Функция send() объекта objHC05 способна отправлять массивы и переменные любых типов, и так же возвращает true или false, сообщая о результате приёма данных вторым bluetooth модулем. В нашем случае данную функцию нет необходимости вызывать в условии оператора while(), так как эта функция и так постоянно вызывается в коде цикла loop().
С подробным описанием всех функций объекта objHC05 библиотеки iarduino_Bluetooth_HC05 можно ознакомиться на странице Wiki - Trema-модуль bluetooth HC-05.
Код программы для «Дройдика»:
Так как Trema-модуль Bluetooth HC-05 подключён к аппаратной шине UART, то перед загрузкой данного скетча нужно отсоединить провод, либо от вывода TX модуля, либо от вывода RX на плате Tream-Power Shield, а после загрузки скетча, подсоединить обратно.
// Подключаем библиотеки: //
#include <Servo.h> // Подключаем библиотеку Servo для работы с сервоприводами
#include <iarduino_HC_SR04_int.h> // Подключаем библиотеку iarduino_HC_SR04_int для работы с ультразвуковым датчиком HC-SR04+ (ссылка на библиотеку: https://iarduino.ru/file/283.html).
#include <iarduino_Bluetooth_HC05.h> // Подключаем библиотеку iarduino_Bluetooth_HC05 для работы с Trema Bluetooth модулем HC-05 (ссылка на библиотеку: https://iarduino.ru/file/301.html).
// Определяем номера выводов: //
const uint8_t pinEcho = 2; // Определяем константу с номером вывода подключённым к выводу Echo датчика расстояний (можно указывать только те выводы Arduino, которые могут работать с внешними прерываниями)
const uint8_t pinTrig = 3; // Определяем константу с номером вывода подключённым к выводу Trig датчика расстояний (может быть любым)
const uint8_t pinLeftTop = 4; // Определяем константу с номером вывода подключённым к верхнему сервоприводу левой ноги (может быть любым)
const uint8_t pinRightTop = 5; // Определяем константу с номером вывода подключённым к верхнему сервоприводу правой ноги (может быть любым)
const uint8_t pinLeftBot = 6; // Определяем константу с номером вывода подключённым к нижнему сервоприводу левой ноги (может быть любым)
const uint8_t pinRightBot = 7; // Определяем константу с номером вывода подключённым к нижнему сервоприводу правой ноги (может быть любым)
const uint8_t pinKey = 9; // Определяем константу с номером вывода подключённым к кнопке (может быть любым)
const uint8_t pinBTKey = 10; // Определяем константу с номером вывода подключённым к выводу K модуля bluetooth (может быть любым)
// Определяем константы: //
const uint8_t cenLeftTop = 100; // Определяем константу центрального угла в градусах для верхнего сервопривода левой ноги (по умолчанию = 100)
const uint8_t cenRightTop = 80; // Определяем константу центрального угла в градусах для верхнего сервопривода правой ноги (по умолчанию = 80 )
const uint8_t cenLeftBot = 60; // Определяем константу центрального угла в градусах для нижнего сервопривода левой ноги (по умолчанию = 60 )
const uint8_t cenRightBot = 120; // Определяем константу центрального угла в градусах для нижнего сервопривода правой ноги (по умолчанию = 120)
const uint8_t maxStepSize = 15; // Определяем константу максимального размера шага в градусах поворота верхних сервоприводов (чем больше угол, тем шире шаг)
const uint8_t maxStepHeight = 10; // Определяем константу максимальной высоты шага в градусах наклона в стороны при ходьбе (чем больше угол, тем выше шаг)
const uint32_t maxSpeed = 1000; // Определяем константу максимальной скорости, она хранит значение задержки в мкс, чем меньше это значение, тем выше максимальная скорость
const uint32_t minSpeed = 10000; // Определяем константу минимальной скорости, она хранит значение задержки в мкс, чем больше это значение, тем ниже минимальная скорость
const uint8_t minDistance = 10; // Определяем константу минимального расстояния в см, при котором робот должен остановиться
// Создаём объекты: //
iarduino_HC_SR04_int objSensor(pinTrig, pinEcho); // Создаём объект sensor для работы с датчиком расстояний, указывая номера выводов Trig и Echo
iarduino_Bluetooth_HC05 objHC05(pinBTKey); // Создаём объект objHC05 для работы с модулем bluetooth, указывая вывод K модуля
Servo objServoLeftTop; // Создаём объект servoLeftTop для работы с верхним левым сервоприводом
Servo objServoRightTop; // Создаём объект servoRightTop для работы с верхним правым сервоприводом
Servo objServoLeftBot; // Создаём объект servoLeftBot для работы с нижним левым сервоприводом
Servo objServoRightBot; // Создаём объект servoRightBot для работы с нижним правым сервоприводом
// Создаём переменные: //
uint8_t valPosition = 224; // Определяем переменную (движение) для хранения текущей позиции шага (счёт от 0 до 255 или обратно), начальная позиция 224
int8_t valTurning = 0; // Определяем переменную (поворот ) для пересчета размера шага в градусах поворота верхних сервоприводов (-10 - влево ... 0 - прямо ... +10 вправо)
uint8_t maxLeftSize = maxStepSize; // Определяем переменную максимального размера шага в градусах поворота верхнего левого сервопривода (чем меньше угол, тем сильнее робот будет уходить вправо)
uint8_t maxRightSize = maxStepSize; // Определяем переменную максимального размера шага в градусах поворота верхнего правого сервопривода (чем меньше угол, тем сильнее робот будет уходить влево )
bool flgPult = false; // Определяем флаг запрещающий чтение данных с пульта более 1 раза за отведённое время
bool flgPosition = false; // Определяем флаг запрещающий изменение позиции шага более 1 раза за отведённое время
uint32_t valSpeed = minSpeed; // Определяем переменную скорости (это задержка изменения позиции шага в мкс, чем меньше значение, тем выше скорость)
// Создаём переменные получения данных с пульта: //
uint8_t sumPultErr = 20; // Определяем переменную для подсчёта количества ошибок чтения данных с пульта
int8_t arrData[3]; // Объявляем массив arrData значения которого будут обновляться по bluetooth, можно создавать массивы или переменные любых типов в т.ч. и char
//
void setup(){ //
// Настраиваем работу с Trema Bluetooth модулем //
pinMode(pinKey, INPUT); // Переводим вывод pinKey (кнопка сопряжения) в режим входа
while(!objHC05.begin(Serial)) {;} // Инициируем работу с bluetooth модулем, указывая имя объекта или класса для управления шиной UART. При провале инициализации функция begin() вернёт false и тогда оператор while запустит её вновь.
while(!objHC05.checkConnect()){ // Проверка подключения к внешнему Bluetooth устройству, до тех пор пока связь не будет установлена...
if(digitalRead(pinKey)){ // Во время проверки наблюдаем не нажата ли кнопка сопряжения, если нажата, то ...
objHC05.createSlave("Droidik","1234"); // Назначаем Bluetooth модулю роль ведомого с именем "Droidik" и PIN-кодом "1234"
while(digitalRead(pinKey)){;} // Ждём пока кнопка сопряжения не будет отпущена
} delay(1000); // Устанавливаем задержку на 1 секунду
} //
// Настраиваем работу с датчиком расстояния: //
objSensor.averaging=50; // Устанавливаем коэффициент усреднения показаний датчика (0-без усреднений, 1-минимальное усреднение, ... 50-высокое усреднение, ...)
} //
void loop(){ //
// ========================================================================== //
// Получаем данные с пульта в массив arrData: //
if(millis()%50<5){ // Каждые 50 мс в течении первых 5 мс ...
if(flgPult){ // Если установлен флаг «flgPult» ...
if( objHC05.available()){ // Если есть принятые данные ...
objHC05.read(arrData); // Читаем полученные данные в массив arrData
sumPultErr=0; // Сбрасываем счётчик ошибок при получении данных с пульта
}else{if(sumPultErr<20){sumPultErr++;}} // Если нет принятых данных, то увеличиваем счётчик ошибок пульта sumPultErr
flgPult=false;} // Сбрасываем флаг «flgPult», чтоб чтение данных выполнилось только один раз за отведённые 5 мс
}else{flgPult=true;} // Если первые 5 мс из очередных 50 мс прошли, то устанавливаем флаг «flgPult» разрешая прочитать новые данные в следующие 50 мс.
// ========================================================================== //
// Читаем показания с датчика расстояния: //
if(objSensor.distance()<=minDistance){ // Если обнаружено препятствие (реальное расстояние до объекта возвращённое функцией sensor.distance() меньше чем указано в константе minDistance), то ...
if(arrData[1]>=0){ // Если джойстик не отклонен назад, то ...
arrData[1]=0; // Запрещаем роботу идти вперёд.
arrData[0]=0; // Запрещаем роботу разворачиваться на месте.
} // Роботу остаётся только идти назад (с заворотом или без).
} //
// ========================================================================== //
// Меняем значения переменных valPosition, valTurning и valSpeed: // Назначение переменных valPosition и valTurning подробно описано ниже, в разделе «Шагаем». А переменная valSpeed определяет скорость с которой эти переменные меняются.
if(micros()%valSpeed<500){ // Каждые valSpeed мкс в течении первых 500 мкс ...
if(flgPosition){ // Если установлен флаг «flgPosition» ...
// Если джойстик отклонён вперёд или назад: //
if(arrData[1]!=0){ // Если джойстик отклонён вперёд arrData[1]>0 или назад arrData[1]<0, то ...
valSpeed = map(abs(arrData[1]), 0, 100, minSpeed, maxSpeed); // Присваиваем переменной valSpeed задержку в мкс пропорционально отклонению джойстика arrData[1] вперёд или назад.
valTurning = arrData[0]/10; // Присваиваем переменной valTurning значение поворота от -10 до +10 пропорционально отклонению джойстика arrData[0] влево или вправо.
if(arrData[1]>0){valPosition++;} // Если джойстик отклонён вперёд arrData[1]>0, то увеличиваем позицию походки varPosition.
else {valPosition--;} // Если джойстик отклонён назад arrData[1]<0, то уменьшаем позицию походки varPosition.
// Если джойстик отклонён только влево или вправо: //
}else if(arrData[0]!=0){ // Иначе (если джойстик не отклонён вперёд или назад), но отклонён влево arrData[0]<0 или вправо arrData[0]>0, то ...
valSpeed = map(abs(arrData[0]), 0, 100, minSpeed, maxSpeed); // Присваиваем переменной valSpeed задержку в мкс пропорционально отклонению джойстика arrData[0] влево или вправо.
valPosition++; // Увеличиваем позицию походки.
if(arrData[0]>0){valTurning= 10;} // Если джойстик отклонён вправо arrData[0]>0, то присваиваем переменной valTurning максимальное значение = 10.
else {valTurning=-10;} // Если джойстик отклонён влево arrData[0]<0, то присваиваем переменной valTurning минимальное значение = -10.
// Стоим на месте: //
}else{ // Иначе (если джойстик не отклонён вперёд или назад, или влево, или вправо), то ...
/* Тут можно прописать действия */ //
/* которые будут выполняться если */ //
/* джойстик пульта не отклонён */ //
} //
flgPosition=false; // Сбрасываем флаг «flgPosition», чтоб чтение данных выполнилось только один раз за отведённые 500 мкс.
}}else{flgPosition=true;} // Если первые 500 мкс из очередных valSpeed мкс прошли, то устанавливаем флаг «flgPosition» разрешая прочитать новые данные в следующие valSpeed мкс.
// ========================================================================== //
// Установка сервоприводов в требуемые позиции (Шагаем): //
// Движение осуществляется в соответствии со значениями переменных valPosition и valTurning.
// Если значение переменной valPosition приращается, то робот будет идти вперёд. Если значение переменной valPosition убывает, то робот будет идти назад. Чем быстрее выполняется приращение/убавление переменной valPosition, тем быстрее шагает робот.
// Значение переменной valTurning управляет поворотом робота, 0-прямо, 1-прямо и чуть правее, (чем выше значение - тем круче поворот), 10 поворот вправо на месте. Отрицательные значения переменной valTurning действуют аналогично положительным, но поворот осуществляется влево.
// Полный шаг каждой ноги разбит на 4 сектора в соответствии со значением переменной valPosition. Каждая нога последовательно выполняет определённое движение для каждого сектора: вверх, вперёд, вниз, назад.
// Если получены данные о нажатии на джойстик arrData[2], то все суставы ног установятся в центральные положения.
if(sumPultErr<20){ // Если данные с пульта приходят, то ...
if(!objServoLeftTop. attached()){objServoLeftTop. attach(pinLeftTop );} // Указываем объекту servoLeftTop работать с выводом pinLeftTop, если он об этом не знает.
if(!objServoRightTop.attached()){objServoRightTop.attach(pinRightTop);} // Указываем объекту servoRightTop работать с выводом pinRightTop, если он об этом не знает.
if(!objServoLeftBot. attached()){objServoLeftBot. attach(pinLeftBot );} // Указываем объекту servoLeftBot работать с выводом pinLeftBot, если он об этом не знает.
if(!objServoRightBot.attached()){objServoRightBot.attach(pinRightBot);} // Указываем объекту servoRightBot работать с выводом pinRightBot, если он об этом не знает.
//
maxRightSize=maxStepSize; if(valTurning<0){maxRightSize=map(valTurning, 0,-10, maxStepSize, 0);} /* Прямо или влево */ // Корректируем значение maxRightSize (размера шага правой ноги) в соответствии со значением valTurning.
maxLeftSize =maxStepSize; if(valTurning>0){maxLeftSize =map(valTurning, 0, 10, maxStepSize, 0);} /* Прямо или вправо */ // Корректируем значение maxLeftSize (размера шага левой ноги) в соответствии со значением valTurning.
//
if(arrData[2]) {objServoLeftTop. write( cenLeftTop ); // Левая нога поворачивается в центр.
objServoRightTop.write( cenRightTop ); // Правая нога поворачивается в центр.
objServoLeftBot. write( cenLeftBot ); // Левая нога наклоняется в центр.
objServoRightBot.write( cenRightBot );}else // Правая нога наклоняется в центр.
if(valPosition<64 ){objServoLeftTop. write(map(valPosition, 0, 63, cenLeftTop - maxLeftSize , cenLeftTop + maxLeftSize )); // Левая нога поворачивается вправо => отходит назад.
objServoRightTop.write(map(valPosition, 0, 63, cenRightTop - maxRightSize , cenRightTop + maxRightSize ));}else // Правая нога поворачивается вправо => выходит вперёд.
if(valPosition<128){objServoLeftBot. write(map(valPosition, 64, 127, cenLeftBot - maxStepHeight , cenLeftBot +(maxStepHeight/2))); // Левая нога наклоняется вправо => переносит центр тяжести с себя на правую ногу, которая станет опорной.
objServoRightBot.write(map(valPosition, 64, 127, cenRightBot -(maxStepHeight/2) , cenRightBot + maxStepHeight ));}else // Правая нога наклоняется вправо => опускается вниз (становится опорной) и поднимает левую ногу.
if(valPosition<192){objServoLeftTop. write(map(valPosition, 128, 191, cenLeftTop + maxLeftSize , cenLeftTop - maxLeftSize )); // Левая нога поворачивается влево => выходит вперёд.
objServoRightTop.write(map(valPosition, 128, 191, cenRightTop + maxRightSize , cenRightTop - maxRightSize ));}else // Правая нога поворачивается влево => отходит назад.
/*valPosition<255*/{objServoLeftBot. write(map(valPosition, 192, 255, cenLeftBot +(maxStepHeight/2) , cenLeftBot - maxStepHeight )); // Левая нога наклоняется влево => опускается вниз (становится опорной) и поднимает правую ногу.
objServoRightBot.write(map(valPosition, 192, 255, cenRightBot + maxStepHeight , cenRightBot -(maxStepHeight/2)));} // Правая нога наклоняется влево => переносит центр тяжести с себя на левую ногу, которая станет опорной.
// ========================================================================== //
// Разрыв связи с пультом: //
}else{ // Если данные с пульта не приходят, то ...
objServoLeftTop. detach(); digitalWrite(pinLeftTop, LOW); // Разрываем связь объекта servoLeftTop с выводом pinLeftTop и устанавливаем на этом выводе уровень логического 0.
objServoRightTop.detach(); digitalWrite(pinRightTop, LOW); // Разрываем связь объекта servoRightTop с выводом pinRightTop и устанавливаем на этом выводе уровень логического 0.
objServoLeftBot. detach(); digitalWrite(pinLeftBot, LOW); // Разрываем связь объекта servoLeftBot с выводом pinLeftBot и устанавливаем на этом выводе уровень логического 0.
objServoRightBot.detach(); digitalWrite(pinRightBot, LOW); // Разрываем связь объекта servoRightBot с выводом pinRightBot и устанавливаем на этом выводе уровень логического 0.
} //
// ========================================================================== //
// Режим сопряжения: //
if(digitalRead(pinKey)){ // Проверяем не нажата ли кнопка сопряжения, если нажата, то ...
objHC05.createSlave("Droidik","1234"); // Назначаем Bluetooth модулю роль ведомого с именем "Droidik" и PIN-кодом "1234".
while(digitalRead(pinKey)){;} // Ждём пока кнопка сопряжения не будет отпущена.
} // Программа продолжит выполняться, но Дройдик никуда не пойдёт, так как модуль не сможет получать новые данные пока мастер не установит с ним соединение.
} //
Значения констант cenLeftTop, cenRightTop, cenLeftBot и cenRightBot должны быть изменены (откалиброваны) на действительные углы сервоприводов в градусах, при которых все суставы робота находятся в центральном положении. Это выполняется с использованием калибровочного скетча, как описано в уроке 38 Сборка «Дройдика».
В данном коде управление роботом осуществляется в три основных этапа: получение данных с пульта; изменение значений переменных valPosition, valTurning и valSpeed; установка сервоприводов в требуемые позиции. А так же в коде присутствуют дополнительные блоки: получение показаний с датчика расстояний; действия при разрыве связи с пультом; вход в режим сопряжения.
- Получение данных с пульта - выполняется один раз за 50 мс.
- Данный блок начинается с оператора «if» в условии которого написано «millis()%50<5». Это условие будет верно в течении 5 мс и ложно в течении оставшихся 45 из 50 мс, далее опять верно в течении 5 мс, и ложно в течении 45 мс, и т.д.
- Далее следует еще один оператор «if» условием которого является флаг «flgPult». Если флаг установлен, то выполнится код в теле оператора «if» где флаг «flgPult» будет сброшен. Этот флаг позволяет выполнять код получения данных с пульта только 1 раз за отведённые 50 мс.
- Последний оператор «if» данного блока в качестве условия принимает результат выполнения функции «objHC05.available()». Условие будет верно только если blurtooth модуль принял данные с пульта, тогда мы их сохраняем в массив «arrData» и сбрасываем счётчик ошибок пульта «sumPultErr». Если условие не выполнено (нет принятых данных), то мы увеличиваем счётчик ошибок пульта «sumPultErr», но счёт ошибок ведётся только до 20.
- Изменение значений переменных valPosition, valTurning и valSpeed - выполняется один раз за «valSpeed» мкс.
- Данный блок начинается с оператора «if» в условии которого написано «micros()%valSpeed<500». Это условие будет верно в течении 500 мкс и ложно в течении оставшегося времени от «valSpeed» мкс, далее опять верно в течении 500 мкс, и опять ложно, и т.д.
- Далее следует еще один оператор «if» условием которого является флаг «flgPosition». Этот флаг действует по аналогии с флагом «flgPult», он позволяет выполнять код изменения значений переменных только 1 раз за отведённые «valSpeed» мкс.
- Переменная «valSpeed» определяет интервал времени через который выполняется данный блок.
- Переменная «valPosition» используется в следующем блоке и определяет позицию шага от 0 до 255.
- Переменная «valTurning» используется в следующем блоке и определяет степень поворота от -10 до +10.
- Значения переменным «valPosition», «valTurning» и «valSpeed» присваиваются в соответствии с принятыми данными о положении джойстика по осям X «arrData[0]» и Y «arrData[1]».
- Установка сервоприводов в требуемые позиции - выполняется при наличии связи с пультом.
- Данный блок начинается с оператора «if» в условии которого написано «sumPultErr<20». Это условие будет верно если хотя бы одна из последних 20 попыток получить данные с пульта увенчалась успехом. Значит пульт включён и можно «шагать».
- Движение осуществляется в соответствии со значениями переменных «valPosition» и «valTurning».
- Полный шаг робота разбит на 256 частей (от 0 до 255) на которые и указывает значение переменной «valPosition».
- Если значение переменной «valPosition» не меняется, то робот стоит на месте.
- Если значение переменной «valPosition» увеличивается, то робот будет идти вперёд (переход от 255 к 0 считается увеличением).
- Если значение переменной «valPosition» убывает, то робот будет идти назад (переход от 0 к 255 считается уменьшением).
- Чем быстрее выполняется увеличение/уменьшение переменной «valPosition», тем быстрее шагает робот.
- Так как в предыдущем блоке переменная «valPosition» либо уменьшается, либо увеличивается, только на 1, значит скорость её уменьшения или увеличения зависит от переменной «valSpeed».
- Значение переменной «valTurning» ограничивает движение левой или правой ноги, что приводит к повороту робота влево или вправо. Переменная может содержать значения от -10 (максимальный поворот влево), 0 (без поворота), до +10 (максимальный поворот вправо).
- Получение показаний с датчика расстояний - выполняется постоянно.
- Данный блок начинается с оператора «if» в условии которого написано «objSensor.distance()<=minDistance». Это условие будет верно если расстояние в см. до препятствия полученное от функции «objSensor.distance()» меньше чем значение константы «minDistance» в см.
- Далее следует еще один оператор «if» условием которого является положение джойстика по оси Y «arrData[1]>=0». Это условие будет верно при любом положении джойстика, кроме его отклонения назад, или назад и в бок.
- Если оба условия выполнены, значит мы пытаемся идти вперёд (с заворотом или без) или развернуться на месте, не смотря на наличие препятствия. В таком случае присваиваем 0-му и 1-му элементам массива «arrData» значение 0, что соответствует центральному положению джойстика по осям X и Y. Тогда остальные блоки кода будут думать что джойстик не отклонён и робот будет стоять на месте.
- Действия при разрыве связи с пультом - выполняется если не выполнялся блок установки сервоприводов.
- Данный блок начинается после оператора «else» в условии оператора «if» которого написано «sumPultErr<20». Значит данный блок будет выполняться если последние 20 попыток чтения данных с пульта увенчались провалом, то есть пульт выключён.
- В этом блоке вызывается функция «detach()» для каждого объекта управления сервоприводом, которая разрывает связь объекта с выводом сервопривода. Далее на каждом выводе сервопривода устанавливается уровень логического 0.
- В результате выполнения кода данного блока, все сервоприводы ослабнут.
- Вход в режим сопряжения - выполняется при нажатии на кнопку сопряжения.
- Данный блок начинается с оператора «if» в условии которого написано «digitalRead(pinKey)». Это условие будет верно если нажата кнопка подключённая ко входу, номер которого указан в константе «pinKey».
- Далее следует вызов функции «createSlave("Droidik","1234");» объекта «objHC05» которая назначает bluetooth модулю роль ведомого с именем "Droidik" и PIN-кодом "1234", разрывает ранее установленную связь с ведомым (если она была) и стирает список ранее созданных пар. После чего модуль начинает ожидать подключение мастера который правильно укажет имя и PIN-код модуля.
- В конце блока вызывается функция «digitalRead(pinKey)» как условие оператора «while», то есть программа входит в цикл пока не будет отпущена кнопка подключённая ко входу, номер которого указан в константе «pinKey» и только потом продолжит выполнение кода. Эта строка предотвращает возможность многократного создания ведомой роли bluetooth модулю за одно нажатие кнопки.
Получение данных и работа с Trema-модулем Bluetooth HC-05 осуществляется через функции и методы объекта objHC05 библиотеки iarduino_Bluetooth_HC05, с подробным описанием которых можно ознакомиться на странице Wiki - Trema-модуль bluetooth HC-05.
Управление:
Сразу после сборки, загрузки скетча и подачи питания на пульт, и «Дройдика», суставы робота будут ослаблены, и он не будет реагировать на команды с пульта, так как Bluetooth модулям требуется сопряжение (создание пары). Сопряжение достаточно выполнить только один раз, bluetooth модули запомнят созданную пару в своей энергонезависимой памяти и будут пытаться соединится друг с другом при каждой последующей подаче питания.
- Отключите питание пульта (если оно было подано), нажмите на джойстик (как на кнопку) и подайте питание пульта. После выполнения этих действий bluetooth модулю пульта будет назначена роль мастера и он начнёт поиск ведомого с именем «Droidik» и PIN-кодом «1234».
- Подключите питание робота (если оно не было подано), нажмите и удерживайте кнопку сопряжения не менее 1 секунды (её можно нажимать в любое время). После нажатия на кнопку, bluetooth модулю робота будет назначена роль ведомого с именем «Droidik» и PIN-кодом «1234», и он будет ожидать подключение мастера.
- Для выполнения повторного сопряжения (если оно потребуется) нужно выполнить те же действия как для пульта, так и для робота.
- Как только связь будет установлена, суставы робота «оживут» и он будет выполнять команды пульта. Если Вы отключите питание пульта, то суставы робота ослабнут и вновь оживут при подаче питания пульта.
Управление роботом с пульта выполняется следующим образом:
- Если отклонить джойстик вперёд, то и робот пойдёт вперёд, а скорость будет зависеть от степени отклонения джойстика.
- Если отклонить джойстик назад, то и робот пойдёт назад, а скорость будет зависеть от степени отклонения джойстика.
- Если отклонить джойстик вперёд и влево, или вправо, то и робот пойдёт вперёд заворачивая влево, или вправо. Скорость будет зависеть от степени отклонения джойстика вперёд, а радиус поворота от степени отклонения джойстика влево, или вправо.
- Если отклонить джойстик назад и влево, или вправо, то и робот пойдёт назад заворачивая влево, или вправо. Скорость будет зависеть от степени отклонения джойстика назад, а радиус поворота от степени отклонения джойстика влево, или вправо.
- Если отклонить джойстик влево или вправо, но не отклонять его вперёд, или назад, то робот начнёт разворачиваться на месте влево, или вправо, а скорость разворота будет зависеть от степени отклонения джойстика.
- Если нажать на джойстик (при включённом питании), то все суставы ног робота установятся в центральные положения.
Примечание:
Так как Trema-модуль Bluetooth HC-05 установленный на «Дройдике» подключён к аппаратной шине UART, то перед загрузкой скетча робота нужно отсоединить провод, либо от вывода TX модуля, либо от вывода RX на плате Tream-Power Shield, а после загрузки скетча, подсоединить его обратно. Не рекомендуется использовать программную шину UART в устройствах управляющих сервоприводами, так как библиотека SoftwareSerial влияет на поведение библиотеки Servo во время передачи данных.
Можно обойтись без добавления кнопки сопряжения к «Дройдику», тогда в скетче нужно выполнять сопряжение автоматически при каждой подаче питания робота и пульта. Делается это следующим образом:
В скетче пульта исключите оператор «if» из предпоследней строки кода setup, оставив только тело оператора:
/* Было так: */ if(varK){while(!objHC05.createMaster("Droidik","1234")){;}} // Если кнопка джойстика нажата при старте ...
/* Стало так: */ while(!objHC05.createMaster("Droidik","1234")){;} // Теперь bluetooth модулю назначается роль мастера при каждом включении пульта!
В скетче робота исключите раздел «Режим сопряжения:» из кода loop, а код setup перепишите так:
void setup(){ //
while( !objHC05.begin(Serial) ){;} // Инициируем работу с bluetooth модулем, указывая имя объекта или класса для управления шиной UART. При провале инициализации функция begin() вернёт false и тогда оператор while запустит её вновь.
while( !objHC05.createSlave("Droidik","1234")){;} // Назначаем Bluetooth модулю роль ведомого с именем "Droidik" и PIN-кодом "1234"
while( !objHC05.checkConnect() ){delay(1000);} // Проверка подключения к внешнему Bluetooth устройству, до тех пор пока связь не будет установлена.
delay(500); //
objSensor.distance(); //
objSensor.distance(); //
} //
После внесения этих изменений к роботу можно не подключать кнопку, а при подаче питания не требуется нажимать на джойстик (или кнопку робота), но установка связи между пультом и роботом будет занимать больше времени.
Обсуждение