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

Дистанционное управление робота "QUADRUPED" по Bluetooth

Введение:

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

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

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

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

Видео:

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

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

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

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

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

Модули Trema Set Shield
Trema-потенциометр (колодка G-V-S) 2 площадка (выводы G-V-A0)
Trema-джойстик (колодка Y-G-V-K-X) 4 площадка (выводы A1-G-V-2-A2)
Trema-bluetooth (колодка RX-G-V-K-TX) 6 площадка (выводы 4-G-V-A3-5)
Пульт bluetooth для управления роботом

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

Схема подключения робота QUADRUPED:

Соберите механику, подключите Tream-Power Shield, сервоприводы, датчик расстояния и откалибруйте робота, как это описано на странице Сборка QUADRUPED. Электроника + Калибровка. Далее на боковые панели установите Bluetooth HC-05 и кнопку, первый модуль подключается к шине UART (в примере используется аппаратная шина UART), а второй к любому выводу Arduino Uno (в примере используется вывод D12).

Датчик расстояния Trema Power Shield
Датчик
HC-SR04+
вывод Echo вывод 2 на белой колодке
вывод Trig вывод 3 на белой колодке
вывод Vcc любой вывод на красной колодке
вывод Gnd любой вывод на чёрной колодке

Вы можете изменить выводы 2 и 3 для подключения датчика HC-SR04+ на любые другие, указав их в скетче при объявлении объекта objSensor.

Сервоприводы Trema Power Shield
1 конечность Горизонтальный сустав (№ 0) вывод 4 на белой колодке
Вертикальный сустав (№ 1) вывод 5 на белой колодке
2 конечность Горизонтальный сустав (№ 2) вывод 6 на белой колодке
Вертикальный сустав (№ 3) вывод 7 на белой колодке
3 конечность Горизонтальный сустав (№ 4) вывод 8 на белой колодке
Вертикальный сустав (№ 5) вывод 9 на белой колодке
4 конечность Горизонтальный сустав (№ 6) вывод 10 на белой колодке
Вертикальный сустав (№ 7) вывод 11 на белой колодке

Вы можете изменить выводы 4-11 для подключения сервоприводов на любые другие, указав их в скетче при объявлении массива объектов pinServo[8]. Трехпроводные шлейфы сервоприводов устанавливаются следующим образом:

  • Оранжевый провод подключается к выводу на белой колодке.
  • Красный провод подключается к выводу на красной колодке.
  • Коричневый провод подключается к выводу на чёрной колодке.
Bluetooth Trema Power Shield
Bluetooth HC-05 вывод RX вывод TX на колодке Serial
вывод TX вывод RX на колодке Serial
вывод K (Key) вывод A0 на белой колодке
вывод V (Vcc) любой вывод на красной колодке
вывод G (GND) любой вывод на чёрной колодке

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

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

  • Вывод K (Key) подключается к выводу на белой колодке.
  • Вывод V (Vcc) подключается к выводу на красной колодке.
  • Вывод G (GND) подключается к выводу на чёрной колодке.
Кнопка Trema Power Shield
Trema-кнопка вывод S (Signal) вывод 12 на белой колодке
вывод V (Vcc) любой вывод на красной колодке
вывод G (GND) любой вывод на чёрной колодке

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

Трёхпроводной шлейф подключённый к выводам S, V, G, устанавливается следующим образом:

  • Вывод S (Signal) подключается к выводу на белой колодке.
  • Вывод V (Vcc) подключается к выводу на красной колодке.
  • Вывод G (GND) подключается к выводу на чёрной колодке.

Представленная ниже схема совпадает со схемой из инструкции по сборке QUADRUPED, но к ней добавились два модуля: bluetooth (подключается к выводам A0, TX и RX) и кнопка (подключается к выводу D12).

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

Элементы схемы: «Э0»...«Э7» - сервоприводы, «Э8» - датчик расстояния, «Э9» - Trema bluetooth модуль, «Э10» 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(5, 4);                           // Создаём объект objSerial указывая выводы (RX, TX) Arduino UNO, можно указывать любые выводы. Вывод RX Arduino подключается к выводу TX модуля, вывод TX Arduino подключается к выводу RX модуля.
iarduino_Bluetooth_HC05 objHC05(A3);                               // Создаём объект 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 uint8_t           pinR = A0;                                 // Определяем константу pinR указывая номер вывода Arduino к которому подключён потенциометр      (аналоговый).
//   Определяем константы задающие люфт для центра джойстика:      //
const uint8_t           gapX = 30;                                 // Изменение положения джойстика на +-gapX от центрального значения по оси X, будет восприниматься как 0
const uint8_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 для хранения состояния кнопки джойстика.
uint16_t                varR;                                      // Объявляем переменную varR для хранения положения ручки потенциометра.
int8_t                  arrData[4];                                // Объявляем массив arrData значения которого будем передавать по bluetooth, можно создавать массивы или переменные любых типов в т.ч. и char
                                                                   //
void setup(){                                                      //
//   Устанавливаем режимы работы объявленных выводов:              //
     pinMode(pinX, INPUT);                                         // Устанавливаем режим работы вывода pinX как вход.
     pinMode(pinY, INPUT);                                         // Устанавливаем режим работы вывода pinY как вход.
     pinMode(pinK, INPUT);                                         // Устанавливаем режим работы вывода pinK как вход.
     pinMode(pinR, INPUT);                                         // Устанавливаем режим работы вывода pinR как вход.
//   Считываем показания с кнопки джойстика:                       // Для входа в режим поиска и сопряжения с ведомым 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("QUADRUPED","1212")){;}} // Если кнопка джойстика нажата при старте, то устанавливаем bluetooth модулю роль мастера и пытаемся подключиться к ведомому с именем "QUADRUPED" и PIN-кодом 1212. Если ведомый не доступен, то функция 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
     varR=analogRead(pinR);                                        // Читаем уровень сигнала со входа pinR в переменную varR
     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
                        varR=map(varR,        0,1024,0, 101);      // Преобразуем значения varR от диапазона 0...1023         к диапазону 0...+100
//   Сохраняем полученные значения в массив и передаём его:        //
     arrData[0]=int8_t(varX);                                      // Положение джойстика по оси X  в диапазоне от -100 до +100 с добавлением люфта по середине.
     arrData[1]=int8_t(varY);                                      // Положение джойстика по оси Y  в диапазоне от -100 до +100 с добавлением люфта по середине.
     arrData[2]=int8_t(varR);                                      // Положение ручки потенциометра в диапазоне от    0 до +100.
     arrData[3]=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 до +100. Состояние кнопки отправляется как число 0 или 1.

Если подать питание с нажатой кнопкой джойстика, то в коде setup выполнится код вызова функции createMaster, которая установит bluetooth модулю роль мастера, инициирует поиск ведомого с именем "QUADRUPED" и PIN-кодом "1212", и если такое ведомое устройство будет доступно, то произойдёт сопряжение и соединение с этим ведомым 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.

Код программы для робота QUADRUPED:

Так как Trema-модуль Bluetooth HC-05 подключён к аппаратной шине UART, то перед загрузкой данного скетча нужно отсоединить провод, либо от вывода TX модуля, либо от вывода RX на плате Tream-Power Shield, а после загрузки скетча, подсоединить обратно.

//                      Подключаем библиотеки:                                            //
#include                <Servo.h>                                                         // Подключаем библиотеку Servo, для работы с сервоприводами.
#include                <iarduino_HC_SR04.h>                                              // Подключаем библиотеку iarduino_HC_SR04, для работы с ультразвуковым датчиком расстояния HC-SR04+ (ссылка на библиотеку: https://iarduino.ru/file/17.html).
#include                <iarduino_Bluetooth_HC05.h>                                       // Подключаем библиотеку iarduino_Bluetooth_HC05 для работы с Trema Bluetooth модулем HC-05 (ссылка на библиотеку: https://iarduino.ru/file/301.html).
//                      Объявляем объекты для работы с библиотеками:                      //
iarduino_Bluetooth_HC05 objHC05(A0);                                                      // Создаём объект objHC05   указывая любой вывод Arduino, который подключается к выводу K модуля
iarduino_HC_SR04        objSensor(3, 2);                                                  // Создаем объект objSensor указывая номера выводов к которым подключён датчик расстояния HC-SR04+ (Trig к выводу 3, Echo к выводу 2)
Servo                   objServo[8];  /*  0    1    2    3    4    5    6    7   */       // Создаём массив, каждый элемент которого является объектом библиотеки Servo. Цифры в комментарии указывают на № сервопривода
//                      Определяем сервы: Г    В    Г    В    Г    В    Г    В            // Буквы в комментарии указывают на плоскость вращения сервопривода (Горизонтальная / Вертикальная)
const uint8_t           pinServo[8] = {   4,   5,   6,   7,   8,   9,  10,  11   };       // Определяем массив хранящий номера выводов к которым подключены сервоприводы (можно менять)
const int               cenAngle[8] = {  90,  90,  90,  90,  90,  90,  90,  90   };       // Определяем массив хранящий углы в градусах, при которых сервоприводы находятся в центральном положении (КОРРЕКТИРУЕТСЯ В КАЛИБРОВОЧНОМ СКЕТЧЕ)
const int               minAngle[8] = {  50,  30,  50,  30,  50,  30,  50,  30   };       // Определяем массив хранящий углы в градусах, на которые отклоняются сервоприводы от центрального положения назад  или вниз
const int               maxAngle[8] = {  50,  40,  50,  40,  50,  40,  50,  40   };       // Определяем массив хранящий углы в градусах, на которые отклоняются сервоприводы от центрального положения вперёд или вверх
//                      Определяем константы:                                             //
const uint8_t           pinK        = 12;                                                 // Определяем константу pinK указывая номер вывода Arduino к которой подключена кнопка сопряжения
const uint32_t          minDelay    = 1000;                                               // Задержка для максимальной скорости (чем ниже значение тем выше скорость)
const uint32_t          maxDelay    = 10000;                                              // Задержка для минимальной скорости  (чем ниже значение тем выше скорость)
//                      Объявляем функции:                                                //
void                    funLimbMove   ( uint8_t, bool, int8_t );                          // Объявляем Функцию установки  одного сустава     в значение от -100 до 100 (конечность 1-4, сустав 0/1, положение -100...+100)
void                    funLimbStep   ( uint8_t, uint8_t, int, int );                     // Объявляем Функцию установки  одной  конечности  в положение от 0 до 255   (конечность 1-4, положение 0...255, ограничение по горизонтали -100...+100, ограничение по вертикали 0...100%)
void                    funLimbGait   ( uint8_t, uint8_t, int, int );                     // Объявляем Функцию установки  всех   конечностей в положение от 0 до 255   (тип походки 0-4, положение 0...255, ограничение по горизонтали -100...+100, ограничение по вертикали 0...100%)
void                    funLimbFree   ( void );                                           // Объявляем Функцию ослабления всех   суставов                              (без параметров)
void                    funLimbCent   ( void );                                           // Объявляем Функцию установки  всех   суставов в центральное положение      (без параметров)
void                    funWaitMaster ( void );                                           // Объявляем Функцию удаления списка пар и ожидания мастера в роли ведомого  (без параметров)
//                      Объявляем массив для получения данных от мастера:                 // 
int8_t                  arrData[4];                                                       // Объявляем массив arrData значения которого будут обнавляться по bluetooth, можно создавать массивы или переменные любых типов в т.ч. и char
//                      Определяем дополнительные переменные:                             //
uint8_t                 varPosition = 0;                                                  // Текущая позиция походки от 0 до 255 (чем быстрее меняется значение тем выше скорость, если значение увеличивается - значит вперёд, если значение уменьшается - значит назад)
bool                    f           = false;                                              // Флаг запрещающий чтение данных более 1 раза за отведённое время
uint8_t                 sumPultErr  = 20;                                                 // Количество ошибок чтения данных с пульта
                                                                                          //
void setup(){                                                                             //
     funLimbFree();                                                                       // Ослабляем все суставы
     pinMode(pinK, INPUT);                                                                // Переводим вывод pinK (кнопка сопряжения) в режим входа
     while( !objHC05.begin(Serial)  ){;}                                                  // Инициируем работу с bluetooth модулем, указывая имя объекта или класса для управления шиной UART. При провале инициализации функция begin() вернёт false и тогда оператор while запустит её вновь.
     while( !objHC05.checkConnect() ){funWaitMaster(); delay(1000);}                      // Проверка подключения к внешнему Bluetooth устройству. Если во время проверки нажата кнопка сопряжения, то устанавливаем bluetooth модулю роль ведомого с именем "QUADRUPED" и PIN-кодом 1212, ожидающего подключение мастера (список сопряжённых пар будет очищен).
}                                                                                         //
                                                                                          //
void loop(){                                                                              //
//   Получаем данные с пульта (один раз за каждые 50 мс):                                 //
     if(millis()%50<5){                                                                   // Каждые 50мс в течении первых 5мс ...
         if(f){                                                                           // Если установлен флаг «f» ...
             if(objHC05.available()){                                                     // Если есть принятые данные ...
                 objHC05.read(arrData);                                                   // Читаем полученные данные в массив arrData
                 sumPultErr=0;                                                            // Сбрасиваем счётчик ошибок при получении данных с пульта
             }else{if(sumPultErr<20){sumPultErr++;}}                                      // Если нет принятых данных, то увеличиваем счёткик ошибок пульта sumPultErr
             f=false;                                                                     // Сбрасываем флаг «f», чтоб чтение данных выполнилось только один раз за отведённые 5мс
         }                                                                                //
     }else{f=true;}                                                                       // Если первые 5мс из очередных 50мс прошли, то устанавливаем флаг «f».
//   Проверяем наличие препятствий (один раз за полный шаг):                              //
     if(varPosition==0){                                                                  // Один раз за полный шаг (один шаг это полный цикл изменений varPosition от 0 до 255)
     if(objSensor.distance()<=15){                                                        // Если расстояние до препятствия меньше 15 см ...
     if(arrData[1]>0){                                                                    // Если ждойстик отклонён вперёд ...
         arrData[1]=0;                                                                    // Делаем вид, что ждойстик не отклонён вперед
         if(arrData[0]!=0){arrData[0];}                                                   // Если джойстик был откронён еще и влево или вправо, то тоже делаем вид, что ждойстик не отклонён влево или вправо
     }}}                                                                                  //
//   Шагаем:                                                                              //
     if(sumPultErr<20){                                                                   // Если данные с пульта приходят, то ...
//       Вперёд или назад:                                                                //
         if(arrData[1]!=0){                                                               // Если джойстик отклонён вперёд arrData[1]>0 или назад arrData[1]<0, то ...
             if(arrData[1]>0){varPosition++;}                                             // Если джойстик отклонён вперёд, то увеличиваем позицию походки varPosition.
             else            {varPosition--;}                                             // Если джойстик отклонён назад,  то уменьшаем   позицию походки varPosition.
             delayMicroseconds(map(abs(arrData[1]),1,100,maxDelay,minDelay));             // Скорость: устанавливаем задержку в зависимости от степени отклонения джойстика вперёд или назад: abs(arrData[1]) без знака. Чем дальше отклонён джойстик, тем меньше задержка, следовательно, выше скорость.
             funLimbGait(1, varPosition, arrData[0], arrData[2]);                         // Обращаемся к функции выполнения походки funLimbGait, аргументы: походка №1, позиция varPosition, поворот arrData[0], высота arrData[2].
//       Разворот:                                                                        //
         }else if(arrData[0]!=0){                                                         // Иначе (если джойстик не отклонён вперёд или назад), но отклонён влево arrData[0]<0 или вправо arrData[0]>0, то ...
             if(arrData[0]>0){varPosition++;}                                             // Если джойстик отклонён вправо, то увеличиваем позицию походки varPosition.
             else            {varPosition--;}                                             // Если джойстик отклонён влево,  то уменьшаем   позицию походки varPosition.
             delayMicroseconds(map(abs(arrData[0]),1,100,maxDelay,minDelay));             // Скорость: устанавливаем задержку в зависимости от степени отклонения джойстика влево или вправо: abs(arrData[0]) без знака. Чем дальше отклонён джойстик, тем меньше задержка, следовательно, выше скорость.
             funLimbGait(0, varPosition, 0, arrData[2]);                                  // Обращаемся к функции выполнения походки funLimbGait, аргументы: походка №0 (разворот), позиция varPosition, любое значение, высота arrData[2].
//       Стоим на месте:                                                                  //
         }else{                                                                           // Иначе (если джойстик не отклонён вперёд или назад, или влево, или вправо) ...
             funLimbMove(1, 1, map(arrData[2],0,100,+100,-100));                          // Обращаемся к функции установки одного сустава конечности funLimbMove, аргументы: 1-номер конечности, 1-вертикальный сустав, arrData[2]-высота сустава.
             funLimbMove(2, 1, map(arrData[2],0,100,+100,-100));                          // Обращаемся к функции установки одного сустава конечности funLimbMove, аргументы: 2-номер конечности, 1-вертикальный сустав, arrData[2]-высота сустава.
             funLimbMove(3, 1, map(arrData[2],0,100,+100,-100));                          // Обращаемся к функции установки одного сустава конечности funLimbMove, аргументы: 3-номер конечности, 1-вертикальный сустав, arrData[2]-высота сустава.
             funLimbMove(4, 1, map(arrData[2],0,100,+100,-100));                          // Обращаемся к функции установки одного сустава конечности funLimbMove, аргументы: 4-номер конечности, 1-вертикальный сустав, arrData[2]-высота сустава.
         }                                                                                //
//   Если нет данных от пульта:                                                           //
     }else{ funLimbFree(); }                                                              // Если пульт отключился то ослабляем все суставы
//   Устанавливаем все конечности в центральные положения при нажатии на джойстик:        //
     if(arrData[3]){ funLimbCent(); }                                                     // Если нажат джойстик пульта, устанавливаем все конечности в центральные положения
//   Режим сопряжения с мастером:                                                         //
     funWaitMaster();                                                                     // Если нажата кнопка сопряжения, то устанавливаем bluetooth модулю роль ведомого с именем "QUADRUPED" и PIN-кодом 1212, ожидающего подключение мастера (список сопряжённых пар будет очищен).
}                                                                                         //
                                                                                          //
// ====================================================================================== //
                                                                                          //
//   Функция установки одного сустава конечности в значение от -100 до 100:               //
void funLimbMove(uint8_t num, bool joint, int8_t pos){                                    // Аргументы функции: «num» - номер конечности от 1 до 4 , «joint» - тип сустава 0 (горизонтальный) или 1 (вертикальный) , «pos» - положение сустава от -100 (внизу-сзади) до +100 (вверху-спереди)
     uint8_t   i = (num-1) * 2 + joint;                                                   // Определяем № сервопривода (i) по № конечности (num-1) и типу сустава (joint)
     int       k = 0, j = pos? cenAngle[i]:0;                                             // Определяем переменные (j) и (k) для хранения экстремумов
     if(pos>0){k = +100; if( num%2==0||i%2==1 ){j-=maxAngle[i];}else{j+=maxAngle[i];}}    // Находим (k) - максимально допустимое значение для аргумента (pos). Находим (j) - максимально допустимый угол в градусах.
     if(pos<0){k = -100; if( num%2==1&&i%2==0 ){j-=minAngle[i];}else{j+=minAngle[i];}}    // Находим (k) - минимально  допустимое значение для аргумента (pos). Находим (j) - минимально  допустимый угол в градусах.
     if(!objServo[i].attached()){objServo[i].attach(pinServo[i]);}                        // Подключаем объект (objServo) работающий с сервоприводом (i) к выводу (pinServo[i])
         objServo[i].write(map(pos, 0, k, cenAngle[i], j));                               // Устанавливаем сервопривод (i) в угол находящийся между центром (cenAngle[I]) и экстремумом (j)
}                                                                                         //
//   Функция установки одной конечности в положение от 0 до 255:                          //
void funLimbStep(uint8_t num, uint8_t pos, int hor, int ver){                             // Аргументы функции: «num» - номер конечности от 1 до 4 , «pos» - позиция от 0 до 255 , «hor» - ограничение поворота горизонтального сустава от -100 до +100 , «ver» - ограничение высоты вертикального сустава от 0 до 100%
     int i, j;                                                                            // Объявляем переменные (i) - для
     if(pos < 225){i = map(pos,   0, 212, +100, -100); }else                              // Сустав конечности поворачивается назад                            (+100 >>> -100)
                  {i = map(pos, 225, 255, -100, +100); }                                  // Сустав конечности поворачивается вперёд                           (-100 >>> +100)
     if(pos < 225){j =                    -100;        }else                              // Сустав конечности опущен                                          (-100)
     if(pos < 235){j = map(pos, 225, 235, -100, +100); }else                              // Сустав конечности поднимается вверх                               (-100 >>> +100)
     if(pos < 245){j =                    +100;        }else                              // Сустав конечности поднят                                          (+100)
                  {j = map(pos, 245, 255, +100, -100); }                                  // Сустав конечности опускается вниз                                 (+100 >>> -100)
     if(hor<0 && num%2==1){ i = map(i, -100, +100, -(100+hor)  , 100+hor);}               // Ограничиваем  угол (i) горизонтального сустава левых  конечностей (поворот влево)
     if(hor>0 && num%2==0){ i = map(i, -100, +100, -(100-hor)  , 100-hor);}               // Ограничиваем  угол (i) горизонтального сустава правых конечностей (поворот вправо)
                            j = map(j, -100, +100, ver*(-2)+100, 100    );                // Ограничиваем  угол (j) вертикального   сустава любых  конечностей (высота хексапода)
     funLimbMove(num, 0, i);                                                              // Устанавливаем угол (i) для горизонтального (0) сустава конечности (num)
     funLimbMove(num, 1, j);                                                              // Устанавливаем угол (j) для вертикального   (1) сустава конечности (num)
}                                                                                         //
//   Функция выполнения походки по одному из возможных вариантов i                        //
void funLimbGait(uint8_t num, uint8_t pos, int hor, int ver){                             // Аргументы функции: (num) - номер походки от 0 до 2 , (pos) - позиция от 0 до 255 , «hor» - ограничение поворота горизонтальных суставов от -100 до +100 , «ver» - ограничение высоты вертикального сустава от 0 до 100%
     switch(num){                                                                         //
//       Походка номер 0: разворот на месте:                                              // pos = 0         63       127       191       255
         case 0:                                                                          //       |         |         |         |         |
             funLimbStep(1,  31+pos, hor, ver);                                           //  L    | > > > > | > > > > | > > > > | <<< > > | конечность №1 выполняет полный цикл движений (от 255 до 0) быстро возвращаясь в последней тетраде
             funLimbStep(2,   0-pos, hor, ver);                                           //  R    | >>> < < | < < < < | < < < < | < < < < | конечность №2 выполняет полный цикл движений (от 0 до 255) быстро возвращаясь в первой тетраде
             funLimbStep(3,  95+pos, hor, ver);                                           //  L    | > > > > | > > > > | <<< > > | > > > > | конечность №3 выполняет полный цикл движений (от 255 до 0) быстро возвращаясь в предпоследней тетраде
             funLimbStep(4,  63-pos, hor, ver);                                           //  R    | < < < < | >>> < < | < < < < | < < < < | конечность №4 выполняет полный цикл движений (от 0 до 255) быстро возвращаясь во второй тетраде
         break;                                                                           //       |         |         |         |         |
//       Походка номер 1: движение вперёд или назад:                                      // pos = 0         63       127       191       255
         case 1:                                                                          //       |         |         |         |         |
             funLimbStep(1,   0+pos, hor, ver);                                           //  L    | > > > > | > > > > | > > > > | > > <<< | конечность №1 выполняет полный цикл движений (от 0 до 255) быстро возвращаясь в последней тетраде
             funLimbStep(2, 127+pos, hor, ver);                                           //  R    | > > > > | > > <<< | > > > > | > > > > | конечность №2 выполняет полный цикл движений (от 0 до 255) быстро возвращаясь во второй тетраде
             funLimbStep(3,  63+pos, hor, ver);                                           //  L    | > > > > | > > > > | > > <<< | > > > > | конечность №3 выполняет полный цикл движений (от 0 до 255) быстро возвращаясь в предпоследней тетраде
             funLimbStep(4, 191+pos, hor, ver);                                           //  R    | > > <<< | > > > > | > > > > | > > > > | конечность №4 выполняет полный цикл движений (от 0 до 255) быстро возвращаясь в первой тетраде
         break;                                                                           //       |         |         |         |         |
//       Походка номер 2: плывёт вперёд или назад:                                        // pos = 0         63       127       191       255
         case 2:                                                                          //       |         |         |         |         |
             funLimbStep(1,   0+pos, hor, ver);                                           //  L    | > > > > | > > > > | > > > > | > > <<< | конечность №1 выполняет полный цикл движений (от 0 до 255) быстро возвращаясь в последней тетраде
             funLimbStep(2,   0+pos, hor, ver);                                           //  R    | > > > > | > > > > | > > > > | > > <<< | конечность №2 выполняет полный цикл движений (от 0 до 255) быстро возвращаясь в последней тетраде
             funLimbStep(3,   0+pos, hor, ver);                                           //  L    | > > > > | > > > > | > > > > | > > <<< | конечность №3 выполняет полный цикл движений (от 0 до 255) быстро возвращаясь в последней тетраде
             funLimbStep(4,   0+pos, hor, ver);                                           //  R    | > > > > | > > > > | > > > > | > > <<< | конечность №4 выполняет полный цикл движений (от 0 до 255) быстро возвращаясь в последней тетраде
         break;                                                                           //       |         |         |         |         |
}    }                                                                                    //
//   Функция освобождения конечностей:                                                    // Аргументы функции: отсутствуют
void funLimbFree(void){for(uint8_t i=0;i<8;i++){objServo[i].detach();digitalWrite(pinServo[i],LOW);}}
//   Функция установки всех суставов конечностей в центральное положение:                 // Аргументы функции: отсутствуют
void funLimbCent(void){for(uint8_t i=1;i<=4;i++){funLimbMove(i,0,0);funLimbMove(i,1,0);}} //
//   Функция удаления списка пар и ожидания мастера в роли ведомого при нажатии кнопки    // Аргументы функции: отсутствуют
void funWaitMaster(void){if(digitalRead(pinK)){objHC05.createSlave("QUADRUPED","1212"); while(digitalRead(pinK)){;}}}

Ссылка для скачивания скетча.

Значения массива cenAngle должны быть изменены (откалиброваны) на действительные углы сервоприводов в градусах, при которых все суставы робота находятся в центральном положении. Это выполняется с использованием калибровочного скетча, как описано на странице Сборка QUADRUPED. Электроника + Калибровка.

В данном коде управление роботом осуществляется через функции:

  • funLimbMove - установка одного сустава выбранной конечности в значение от -100 до 100.
    • Если указать funLimbMove(4,1,100), то у 4 конечности, вертикальный сустав, поднимется максимально вверх.
    • Если указать funLimbMove(4,0,-100), то у 4 конечности, горизонтальный сустав, сдвинется максимально влево.
  • funLimbStep - установка одной конечности в положение от 0 до 255.
    • Если указать funLimbStep(4,x,0,50), где x будет увеличиваться от 0 до 255, то 4 конечность совершит следующие действия: плавный сдвиг внизу назад, резкий подъём, быстрый проход сверху вперёд и резкий спуск, остановившись в той же позиции с которой начинались движения. Что похоже на реальное движение лап при движении вперёд.
    • Если указать funLimbStep(4,x,0,50), где x будет уменьшаться от 255 до 0, то 4 конечность совершит те же действия, но в обратном порядке. Что похоже на реальное движение лап при движении назад.
    • Предпоследний аргумент функции можно указывать в пределах от -100 до +100, он будет ограничивать амплитуду движений левых или правых конечностей, что вызовет заворачивание (поворот) влево (-100...0) или вправо (0...+100).
    • Последний аргумент функции можно указывать в пределах от 0 до 100, это высота подъёма корпуса в процентах, если указать 0, то QUADRUPED будет «ползти», чем больше значение, тем выше QUADRUPED. Средний подъём корпуса соответствует значению 50.
  • funLimbGait - установка всех конечностей в положение от 0 до 255. Эта функция вызывает предыдущую функцию funLimbStep для каждой конечности, указывая их положение в соответствии с алгоритмом походки.
    • Если указать funLimbGait(1, x, 0, 50), где x будет увеличиваться от 0 до 255, то все конечности совершат действия при которых QUADRUPED пройдёт вперёд и прямо на один полный шаг, а его конечности вернутся в ту же позицию с которой этот шаг начинался.
    • Если указать funLimbGait(1, x, 0, 50), где x будет уменьшаться от 255 до 0, то все конечности совершат те же действия, но в обратном порядке. Следовательно, QUADRUPED пройдёт тот же шаг но назад.
    • Если указать funLimbGait(0, x, 0, 50), где x будет увеличиваться от 0 до 255, то все конечности совершат действия при которых QUADRUPED выполнит разворот вправо на один полный шаг, а его конечности вернутся в ту же позицию с которой этот разворот начинался.
    • Последний и предпоследний аргументы выполняют те же действия что и аргументы функции funLimbStep той же позиции, а именно, выполняют заворот (поворот) и поднятие корпуса.
    • Вы можете дополнить функцию funLimbGait придумав свои варианты походок.
  • funLimbFree - ослабление всех суставов. Функция вызывается без параметров и приводит к отключению сервоприводов. Сервоприводы включатся самостоятельно при вызове любой функции управления суставами или конечностями.
  • funLimbCent - установка всех суставов в центральное положение. Функция вызывается без параметров и устанавливает суставы всех конечностей в положение установленное при калибровке.
  • funWaitMaster - эта функция проверяет нажатие на кнопку сопряжения. Если кнопка нажата, то текущее соединение (если оно есть) с мастером будет разорвано, список пар будет очищен, bluetooth модулю будет назначена роль ведомого с указанием имени «QUADRUPED» и PIN-кодом «1212», после чего он будет ожидать подключения мастера.

Получение данных и работа с Trema-модулем Bluetooth HC-05 осуществляется через функции и методы объекта objHC05 библиотеки iarduino_Bluetooth_HC05, с подробным описанием которых можно ознакомиться на странице Wiki - Trema-модуль bluetooth HC-05.

Управление:

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

  • Отключите питание пульта (если оно было подано), нажмите на джойстик (как на кнопку) и подайте питание пульта. После выполнения этих действий bluetooth модулю пульта будет назначена роль мастера и он начнёт поиск ведомого с именем «QUADRUPED» и PIN-кодом «1212».
  • Подключите питание робота (если оно не было подано), нажмите и удерживайте кнопку сопряжения не менее 1 секунды (её можно нажимать в любое время). После нажатия на кнопку, bluetooth модулю робота будет назначена роль ведомого с именем «QUADRUPED» и PIN-кодом «1212», и он будет ожидать подключение мастера.
  • Для выполнения повторного сопряжения (если оно потребуется) нужно выполнить те же действия как для пульта, так и для робота.
  • Как только связь будет установлена, суставы робота «оживут» и он будет выполнять команды пульта. Если Вы отключите питание пульта, то суставы робота ослабнут и вновь оживут при подаче питания пульта.

Управление роботом с пульта выполняется следующим образом:

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

Примечание:

Так как Trema-модуль Bluetooth HC-05 установленный на роботе, подключён к аппаратной шине UART, то перед загрузкой скетча робота нужно отсоединить провод, либо от вывода TX модуля, либо от вывода RX на плате Tream-Power Shield, а после загрузки скетча, подсоединить его обратно. Не рекомендуется использовать программную шину UART в устройствах управляющих сервоприводами, так как библиотека SoftwareSerial влияет на поведение библиотеки Servo во время передачи данных.

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

В скетче пульта исключите оператор «if» из предпоследней строки кода setup, оставив только тело оператора:

/* Было так:  */ if(varK){while(!objHC05.createMaster("QUADRUPED","1212")){;}} // Если кнопка джойстика нажата при старте ...
/* Стало так: */          while(!objHC05.createMaster("QUADRUPED","1212")){;}  // Теперь bluetooth модулю назначается роль мастера при каждом включении пульта!

В скетче робота исключите целиком все строки в которых встречается константа «pinK» или функция «funWaitMaster()», а код setup() перепишите так:

void setup(){                                                                             //
     funLimbFree();                                                                       // Ослабляем все суставы
     while( !objHC05.begin(Serial)                  ){;}                                  // Инициируем работу с bluetooth модулем, указывая имя объекта или класса для управления шиной UART. При провале инициализации функция begin() вернёт false и тогда оператор while запустит её вновь.
     while( !objHC05.createSlave("QUADRUPED","1212")){;}                                  // Устанавливаем bluetooth модулю роль ведомого с именем "QUADRUPED" и PIN-кодом 1212, ожидающего подключение мастера (список сопряжённых пар будет очищен).
     while( !objHC05.checkConnect()                 ){delay(1000);}                       // Проверка подключения к внешнему Bluetooth устройству.
}                                                                                         //

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

Ссылки:




Обсуждение

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