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

Введение:

В этом уроке мы дополним робота «Дройдика» модулем 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 все модули устанавливаются без проводов, а наличие на каждой площадке всего двух колодок с разным количеством выводов, гарантирует правильность установки модулей.

Схема подключения робота «Дройдика»:

Соберите механику, подключите Tream-Power Shield, сервоприводы, датчик расстояния и откалибруйте робота, как это описано в уроке 38 Сборка «Дройдика». Далее на боковые панели установите Bluetooth HC-05 и кнопку, первый модуль подключается к шине UART (в примере используется аппаратная шина UART), а второй к любому выводу (в примере используется вывод D9).

Вы можете изменить вывод 3 используемый для подключения вывода Trig на любой другой, указав его при определении константы pinTrig. Но вывод Echo датчика HC-SR04+ можно подключать только к выводам с аппаратным прерыванием (2 или 3) так как в скетче используется библиотека iarduino_HC_SR04_int (если использовать библиотеку iarduino_HC_SR04, то и вывод Echo можно будет подключать к любым выводам).

Вы можете изменить выводы 4-7 для подключения сервоприводов на любые другие, указав их в скетче при определении констант pinLeftTop, pinRightTop, pinLeftBot, pinRightBot.

Трехпроводные шлейфы сервоприводов устанавливаются следующим образом:

• Оранжевый провод подключается к выводу на белой колодке.
• Красный провод подключается к выводу на красной колодке.
• Коричневый провод подключается к выводу на чёрной колодке.

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

Выводы RX и ТХ модуля подключаются проводами к выводам TX и RX колодки с надписью Serial. Трёхпроводной шлейф подключённый к выводам K, V, G, устанавливается следующим образом:

• Вывод K (Key) подключается к выводу на белой колодке.
• Вывод 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 (ссылка на библиотеку: /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+  (ссылка на библиотеку: /file/283.html).
#include                <iarduino_Bluetooth_HC05.h>                             //  Подключаем библиотеку iarduino_Bluetooth_HC05 для работы с Trema Bluetooth модулем HC-05     (ссылка на библиотеку: /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();                                                    //
}                                                                            //

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

Ссылки:

• Урок 38: Сборка «Дройдика».
• Библиотека iarduino_Bluetooth_HC05.
• Библиотека iarduino_HC_SR04_int.
• Wiki - Установка библиотек в Arduino IDE.
• Wiki - Trema-модуль bluetooth HC-05.
• Код программы для пульта.
• Код программы для робота.