КОРЗИНА
магазина
8 (499) 500-14-56 | ПН. - ПТ. 12:00-18:00
ЛЕСНОРЯДСКИЙ ПЕРЕУЛОК, 18С2, БЦ "ДМ-ПРЕСС"

Дистанционное управление робота «Дройдика» по Bluetooth

Введение:

В этом уроке мы дополним робота «Дройдика» модулем Bluetooth и создадим пульт дистанционного управления на базе аналогичного Bluetooth модуля. Управление направлением и скоростью движения робота будет осуществляться с помощью джойстика, а при обнаружении препятствий «Дройдик» откажется идти прямо, но согласится пойти назад.

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

Скорость и направление движения робота будет зависеть от степени и направления отклонения джойстика. Робот сможет выполнять такие команды как движение вперёд или назад, с заворотом или без, разворот на месте влево или вправо и устанавливать все суставы в центральные положения. При разрыве связи между bluetooth модулями, робот перестанет идти, а все его сервоприводы ослабнут.

Подробно об управлении роботом и сопряжении Bluetooth модулей рассказано ниже, в разделе «Управление».

Видео:

Нам понадобится:

Робот "Дройдик":

Пульт:

Для реализации проекта нам необходимо установить библиотеки:

  • iarduino_Bluetooth_HC05 - для работы с Trema Bluetooth модулем HC-05.
  • iarduino_HC_SR04_int - для работы с ультразвуковым датчиком расстояния HC-SR04+.
  • Библиотеки SoftwareSerial и Servo входят в базовый набор Arduino IDE и не требуют установки.

О том как устанавливать библиотеки, Вы можете ознакомиться на странице Wiki - Установка библиотек в Arduino IDE.

Схема подключения пульта дистанционного управления:

Модули 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)
Пульт bluetooth для управления роботом

Благодаря применению 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).

Сборка робота для управления по bluetooth

Элементы схемы: «Э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();                                                    //
}                                                                            //

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

Ссылки:




Обсуждение

Гарантии и возврат Используя сайт Вы соглашаетесь с условями