Введение:
В этом уроке мы дополним робота QUADRUPED модулем Bluetooth и создадим пульт дистанционного управления на базе аналогичного Bluetooth модуля. Управление направлением и скоростью движения робота будет осуществляться с помощью джойстика, а высота робота будет регулироваться потенциометром. При обнаружении препятствий робот откажется идти прямо, но согласится пойти назад или развернуться.
Bluetooth модуль пульта будет выполнять роль мастера, а Bluetooth модуль робота - роль ведомого. Сопряжение мастера и ведомого достаточно выполнить только один раз. В дальнейшем, при подаче питания робота и питании пульта, устройства будут соединяться самостоятельно.
Скорость и направление движения робота будет зависеть от степени и направления отклонения джойстика. Робот сможет выполнять такие команды как движение вперёд или назад, с заворотом или без, разворот на месте влево или вправо, установка всех суставов в центральные положения, ослабление всех суставов, подъем и опускание корпуса. При разрыве связи между bluetooth модулями пульта и робота, робот перестанет идти, а все его сервоприводы ослабнут.
Подробно об управлении роботом и сопряжении Bluetooth модулей рассказано ниже, в разделе «Управление».
Видео:
Нам понадобится:
- Робот «QUADRUPED»:
- Робот-платформа «QUADRUPED» x1 шт.
- Arduino Uno x1 шт.
- Tream-Power Shield x1 шт.
- Trema-модуль Bluetooth HC-05 x1 шт.
- Trema-модуль кнопка x1 шт.
- Элементы питания:
- Пульт:
- Arduino Uno x1 шт.
- Trema-Set Shield x1 шт.
- Trema-модуль Bluetooth HC-05 x1 шт.
- Trema-модуль джойстик x1 шт.
- Trema-модуль потенциометр x1 шт.
- Элементы питания:
- либо 1 аккумулятор Ni-MH размером крона и переходник под DC-jack.
- либо иной источник питания для Arduino Uno.
Для реализации проекта нам необходимо установить библиотеки:
- iarduino_Bluetooth_HC05 - для работы с Trema Bluetooth модулем HC-05.
- iarduino_HC_SR04 - для работы с ультразвуковым датчиком расстояния HC-SR04+.
- Библиотеки SoftwareSerial и Servo входят в базовый набор Arduino IDE и не требуют установки.
О том как устанавливать библиотеки, Вы можете ознакомиться на странице Wiki - Установка библиотек в Arduino IDE.
Схема подключения пульта дистанционного управления:
- Установите Trema-Set Shield на Arduino Uno.
- Установите Trema-модуль потенциометр на 2 посадочную площадку Trema-Set Shield.
- Установите Trema-модуль джойстик на 4 посадочную площадку Trema-Set Shield.
- Установите Trema-модуль Bluetooth HC-05 на 6 посадочную площадку Trema-Set Shield.
- При желании все установленные на Trema-Set Shield модули можно закрепить, используя нейлоновые винты и стойки.
| Модули | 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) |
Благодаря применению 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).
Элементы схемы: «Э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 устройству.
} //
После внесения этих изменений к роботу можно не подключать кнопку, а при подаче питания не требуется нажимать на джойстик (или кнопку робота), но установка связи между пультом и роботом будет занимать больше времени.
Обсуждение