Введение:
Установка и подключение электроники QUADRUPED, является заключительным этапом сборки.
Видео:
Нам понадобится:
- Arduino UNO х 1шт.
- Trema Power Shield х 1шт.
- Крепежи.
- Источник питания. (подробнее в разделе питание)
Для реализации проекта нам потребуется библиотека Servo, которая входит в стандартный набор библиотек Arduino IDE (её не требуется скачивать).
Схема подключения:
Сначала нужно закрепить Arduino Uno, при помощи крепежей, на Quadruped. Далее на Arduino Uno устанавливается Trema Power Shield, к которому подключаются выводы датчика расстояния и сервоприводов в соответствии с таблицами:
| Датчик расстояния | Trema Power Shield | |
|---|---|---|
| Датчик HC-SR04+ |
вывод Echo | вывод 2 (можно изменить) |
| вывод Trig | вывод 3 (можно изменить) | |
| Сервоприводы | Trema Power Shield | |
|---|---|---|
| 1 конечность | Горизонтальный сустав (№ 0) | вывод 4 (можно изменить) |
| Вертикальный сустав (№ 1) | вывод 5 (можно изменить) | |
| 2 конечность | Горизонтальный сустав (№ 2) | вывод 6 (можно изменить) |
| Вертикальный сустав (№ 3) | вывод 7 (можно изменить) | |
| 3 конечность | Горизонтальный сустав (№ 4) | вывод 8 (можно изменить) |
| Вертикальный сустав (№ 5) | вывод 9 (можно изменить) | |
| 4 конечность | Горизонтальный сустав (№ 6) | вывод 10 (можно изменить) |
| Вертикальный сустав (№ 7) | вывод 11 (можно изменить) | |
- Если Вы подключите датчик расстояния к другим выводам Arduino Uno, то их нужно изменить в строке скетча где создаётся объект objSensor. Первым параметром указывается номер вывода Trig, а вторым параметром, номер вывода Echo:
iarduino_HC_SR04 objSensor(3, 2);
- Если Вы подключите сервоприводы к другим выводам Arduino Uno, то их нужно изменить в строке скетча где определяется массив pinServo. Номер элемента массива соответствует номеру сустава, а значение элемента массива соответствует номеру вывода Arduino Uno к которому подключён сервопривод этого сустава:
const uint8_t pinServo[8] = {4, 5, 6, 7, 8, 9, 10, 11};
СХЕМА ПОДКЛЮЧЕНИЯ РЕДАКТИРУЕТСЯ
Питание:
8 сервоприводов потребляют значительный ток, который не сможет выдать USB порт компьютера. Питание от внешнего источника через Arduino UNO приведёт к значительной нагрузке на её стабилизатор. Вот почему мы предлагаем воспользоваться Trema Power Shield, который снабжен мощным стабилизатором и позволяет выбирать различные схемы питания. Источник питания на 7-12 В постоянного тока подключается к коннектору «VinS» Trema Power Shield (не перепутайте полярность), а джампер (перемычка) устанавливается в положение «Общ. Vin». В качестве источников питания могут быть использованы адаптеры на 7-12 В с током не менее 1 А, а так же батарейные или аккумуляторные отсеки.
Пример кода программы:
// Подключаем библиотеки: //
#include <iarduino_HC_SR04.h> // Подключаем библиотеку iarduino_HC_SR04, для работы с ультразвуковым датчиком расстояния HC-SR04+ ( ссылка на библиотеку: https://iarduino.ru/file/17.html )
#include <Servo.h> // Подключаем библиотеку Servo, для работы с сервоприводами
// Объявляем объекты для работы с библиотеками: //
iarduino_HC_SR04 objSensor (3, 2); // Создаем объект objSensor библиотеки arduino_HC_SR04, указывая номера выводов к которым подключён датчик расстояния 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 }; // Определяем массив хранящий углы в градусах, на которые отклоняются сервоприводы от центрального положения вперёд или вверх
// Объявляем функции устанавливающие сервоприводы в определённые позиции: //
void funLimbMove ( uint8_t, bool, int8_t ); // Объявляем Функцию установки одного сустава в значение от -100 до 100 (конечность 1-4, сустав 0/1, положение -100...+100)
void funLimbStep ( uint8_t, uint8_t ); // Объявляем Функцию установки одной конечности в положение от 0 до 255 (конечность 1-4, положение 0...255)
void funLimbGait ( uint8_t, uint8_t ); // Объявляем Функцию установки всех конечностей в положение от 0 до 255 (тип походки 0-4, положение 0...255)
void funLimbFree ( void ); // Объявляем Функцию ослабления всех суставов (без параметров)
void funLimbCent ( void ); // Объявляем Функцию установки всех суставов в центральное положение (без параметров)
// Определяем переменные отвечающие за направление и высоту: //
int limWidth = 0; // Определяем переменную для выбора направления движения (-100 влево, +100 вправо, 0 прямо)
uint8_t limHeight = 100; // Определяем переменную для ограничения высоты хексапода (0 ползком, 100 на высоте)
// Определяем переменные участвующие только в коде функции loop: //
uint8_t a = 1; // Текущая позиция походки от 0 до 255 (чем быстрее меняется значение тем выше скорость, если значение увеличивается - значит вперёд, если значение уменьшается - значит назад)
uint8_t b = 1; // Варианты движений (походок) от 1 до 9
uint8_t c = 0; // Счетчик остановки
bool d = false; // Флаг разрешающий сделать очередной шаг
bool e = false; // Флаг разрешающий перейти к очередной походке
//
void setup(){ funLimbCent();} // Устанавливаем все конечности в центральные положения
//
void loop(){ //
if(!c){ // Если счётчик установлен в 0, то ...
if(millis() % 3 < 2){if(d){a++; d=false;}}else{d=true;} // Увеличиваем значение переменной (a) на 1 за каждые 3 мс (т.к. переменная типа uint8_t то она сама сбросится в 0 после значения 255)
if(millis() % 5000 < 2){if(e){b++; e=false;}}else{e=true;} // Увеличиваем значение переменной (b) на 1 за каждые 5 сек (по 5 секунд демонстрируется каждый вариант походки)
// Выполняем 1 из 9 предложенных вариантов поведения (в зависимости от значения b) // Устанавливаем значения переменным limWidth, limHeight и вызываем функцию funLimbGait() с различными параметрами
switch(b){ //
case 1: limWidth= 0; limHeight=100; funLimbGait(0, a); break; // Поворачиваемся на месте влево
case 2: limWidth= 0; limHeight=100; funLimbGait(0, 255-a); break; // Поворачиваемся на месте вправо
case 3: limWidth= 0; limHeight=100; funLimbGait(1, a); break; // Идём вперёд походкой 1
case 4: limWidth= 0; limHeight=100; funLimbGait(1, 255-a); break; // Идём назад походкой 1
case 5: limWidth= 0; limHeight= 60; funLimbGait(1, a); break; // Идём вперёд походкой 1 с высотой 60% (пригнувшись)
case 6: limWidth= 0; limHeight= 30; funLimbGait(1, a); break; // Идём вперёд походкой 1 с высотой 30% (ползём)
case 7: limWidth=-10; limHeight=100; funLimbGait(1, a); break; // Идём вперёд походкой 1 незначительно поворачивая вправо
case 8: limWidth= 10; limHeight=100; funLimbGait(1, a); break; // Идём вперёд походкой 1 незначительно поворачивая влево
case 9: funLimbFree(); break; // Отдыхаем
default: b=1; // Начинаем цикл заново
} } //
// Проверяем наличие препятствий: //
if(a==2){ // Если текущая позиция походки равна 2, то ... (проверяем наличие препятствий только один раз за весь цикл походки (a = 0...255), так как ответ датчика HC-SR04+ занимает много времени)
if(objSensor.distance()<=15){c=99; funLimbCent(); delay(100);} // Если расстояние до препятствия меньше 15 см, то устанавливаем счетчик в 99, устанавливаем все суставы в центральное положение и устанавливаем задержку на 100 мс
else {if(c){c--;}} // Если расстояние до препятствия больше 15 см, то если счётчик больше 0, то уменьшаем его значение на 1 пока он не станет равен 0.
} //
} //
//
// ======================================================================================== //
//
// Функция установки одного сустава конечности в значение от -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){ // Аргументы функции: (num) - номер конечности от 1 до 4 , (pos) - позиция от 0 до 255
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, 235, 245, -100, +100); }else // Сустав конечности поднимается вверх (-100 >>> +100)
if(pos < 245){j = +100; }else // Сустав конечности поднят (+100)
{j = map(pos, 245, 255, +100, -100); } // Сустав конечности опускается вниз (+100 >>> -100)
if(limWidth<0 && num%2==0){ i = map(i, -100, +100, -(100+limWidth) , 100+limWidth);} // Ограничиваем угол (i) горизонтального сустава левых конечностей (поворот влево)
if(limWidth>0 && num%2==1){ i = map(i, -100, +100, -(100-limWidth) , 100-limWidth);} // Ограничиваем угол (i) горизонтального сустава правых конечностей (поворот вправо)
j = map(j, -100, +100, limHeight*(-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){ // Аргументы функции: (num) - номер походки от 0 до 2 , (pos) - позиция от 0 до 255
switch(num){ //
// Разворот на месте: // pos = 0 63 127 191 255
case 0: // | | | | |
funLimbStep(1, 31-pos); // L | < < >>> | < < < < | < < < < | < < < < | конечность №1 выполняет полный цикл движений (от 255 до 0) быстро возвращаясь в первой тетраде
funLimbStep(2, 0+pos); // R | > > > > | > > > > | > > > > | > > <<< | конечность №2 выполняет полный цикл движений (от 0 до 255) быстро возвращаясь в последней тетраде
funLimbStep(3, 95-pos); // L | < < < < | < < >>> | < < < < | < < < < | конечность №3 выполняет полный цикл движений (от 255 до 0) быстро возвращаясь во второй тетраде
funLimbStep(4, 63+pos); // R | > > > > | > > > > | > > <<< | > > > > | конечность №4 выполняет полный цикл движений (от 0 до 255) быстро возвращаясь в предпоследней тетраде
break; // | | | | |
// Походка №1 вперёд или назад: // pos = 0 63 127 191 255
case 1: // | | | | |
funLimbStep(1, 0+pos); // L | > > > > | > > > > | > > > > | > > <<< | конечность №1 выполняет полный цикл движений (от 0 до 255) быстро возвращаясь в последней тетраде
funLimbStep(2, 127+pos); // R | > > > > | > > <<< | > > > > | > > > > | конечность №2 выполняет полный цикл движений (от 0 до 255) быстро возвращаясь во второй тетраде
funLimbStep(3, 63+pos); // L | > > > > | > > > > | > > <<< | > > > > | конечность №3 выполняет полный цикл движений (от 0 до 255) быстро возвращаясь в предпоследней тетраде
funLimbStep(4, 191+pos); // R | > > <<< | > > > > | > > > > | > > > > | конечность №4 выполняет полный цикл движений (от 0 до 255) быстро возвращаясь в первой тетраде
break; // | | | | |
// Походка №2 плывёт вперёд или назад: // pos = 0 63 127 191 255
case 2: // | | | | |
funLimbStep(1, 0+pos); // L | > > > > | > > > > | > > > > | > > <<< | конечность №1 выполняет полный цикл движений (от 0 до 255) быстро возвращаясь в последней тетраде
funLimbStep(2, 0+pos); // R | > > > > | > > > > | > > > > | > > <<< | конечность №2 выполняет полный цикл движений (от 0 до 255) быстро возвращаясь в последней тетраде
funLimbStep(3, 0+pos); // L | > > > > | > > > > | > > > > | > > <<< | конечность №3 выполняет полный цикл движений (от 0 до 255) быстро возвращаясь в последней тетраде
funLimbStep(4, 0+pos); // 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);}}//
Управление:
Если воспользоваться предложенным кодом, то управление квадропедом сводится к тому, что в коде loop должна постоянно вызываться функция funLimbGait, которая принимает два параметра: тип походки (значение от 0 до 2) и позиция (значение от 0 до 255). Если значение позиции меняется - квадропед идёт, если не меняется - стоит. Функция содержит 3 типа походки (от 0 до 2) смысл которых заключается в том, что у каждой походки своя последовательность движений конечностями. Каждая походка состоит 256 позиций (от 0 до 255), перебирая всю последовательность позиций, мы заставляем идти квадропеда (как перебирая номера кадров, создаём анимацию), чем быстрее меняется значение позиции - тем быстрее идёт квадропед. Если значение позиции увеличивать, то квадропед пойдёт вперёд, а если уменьшать, то назад.
Например:
if(millis() % 5 == 0){i++;} // Увеличиваем i на 1 за каждые 5 мс (это позиция походки)
if(i>255){i=0;} // Сбрасываем i в 0, т.к. позиция может принимать значение от 0 до 255
funLimbGait(1, i); // Идём вперёд походкой № 1 (все конечности совершат полный цикл своего хода за время пока i увеличивается от 0 до 255, далее i сбросится в 0 и цикл повторится)
if(millis() % 9 == 0){i++;} // Увеличиваем i на 1 за каждые 9 мс (в этом примере значение переменной i увеличивается медленнее)
if(i>255){i=0;} // Сбрасываем i в 0, т.к. позиция может принимать значение от 0 до 255
funLimbGait(1, i); // Идём вперёд походкой № 1 (теперь квадропед будет идти медленнее чем в предыдущем примере)
if(millis() % 5 == 0){i--;} // Уменьшаем i на 1 за каждые 5 мс (в этом примере i не увеличивается, а уменьшается)
if(i<0){i=255;} // Устанавливаем i в 255, т.к. позиция может принимать значение от 0 до 255
funLimbGait(1, i); // Идём назад походкой № 1 (квадропед пойдёт назад, т.к. все суставы будут совершать свои движения в обратном порядке, как при перемотке анимации назад)
Еще есть две переменные: limWidth (принимает значения от -100 до +100) и limHeight (принимает значения от 0 до 100) которые позволяют квадропеду не просто идти при помощи функции funLimbGait, а еще пригибаться и поворачивать. Переменная limWidth отвечает за повороты при ходьбе (-100 - влево, ... , 0 - прямо, ... , +100 - вправо), а переменная limHeight отвечает за высоту квадропеда при ходьбе (0 - ползёт ... 100 - приподнят). В коде loop приведены 9 примеров с использованием некоторых значений переменных limWidth и limHeight, а так же аргументов функции funLimbGait.
Калибровка:
Калибровка выполняется только 1 раз, после сборки квадропеда.
При сборке квадропеда невозможно установить качалки всех сервоприводов с точностью до градуса, вот почему в предложенном скетче имеется массив cenAngle. Номера элементов массива соответствуют номерам суставов, а значения элементов массива являются углами сервоприводов при которых суставы конечностей находятся в центральном положении. Калибровка заключается в том, что бы заменить углы по умолчанию 90°, на те которые действительно соответствуют центральным положениям суставов.
const int cenAngle[8] = {90, 90, 90, 90, 90, 90, 90, 90}; // Определяем массив хранящий углы в градусах, при которых сервоприводы находятся в центральном положении (КОРРЕКТИРУЕТСЯ В КАЛИБРОВОЧНОМ СКЕТЧЕ)
Можно последовательно менять значения каждого элемента массива и загружать скетч с пустым кодом loop, повторяя эту процедуру по несколько раз для каждого сервопривода Вы найдёте значения для всех суставов и заполните массив действительными значениями. Но есть способ проще - воспользоваться калибровочным скетчем, который выдаёт значения всех элементов массива в монитор последовательного порта после каждого сдвига конечности. Загрузите калибровочный скетч и откройте монитор последовательного порта. Вводя числа от 0 до 11 Вы выбираете сустав, а вводя символы + или - Вы сдвигаете выбранный сустав. После каждого сдвига сустава, в мониторе высвечивается строка с указанием текущего угла выбранного сустава и результатом в виде всего массива cenAngle. После установки всех суставов в центральные положения, скопируйте результат, заменив им строку определяющую массив cenAngle основного скетча (представлена выше), сохраните этот (основной) скетч и загрузите его, на этом калибровка будет завершена и калибровочный скетч больше не понадобится.
Код калибровочного скетча:
#include <Servo.h>
Servo objServo[8];
const uint8_t pinServo[8] = { 4, 5, 6, 7, 8, 9, 10, 11};
int cenAngle[8] = {90, 90, 90, 90, 90, 90, 90, 90};
uint8_t i, f, numServo = 0;
void setup(){
Serial.begin(9600);
Serial.println(F("ВВЕДИТЕ:\r\n0...7 - выбор номера сустава\r\n+ - сдвиг сустава вверх или вперёд на 1°\r\n- - сдвиг сустава вниз или назад на 1°\r\n* - освободить все суставы (выключить сервоприводы)\r\n? - подсказка\r\n========================================================"));}
void loop(){
if(Serial.available()>0){
i=f=0;
while(Serial.available()){
switch(Serial.read()){
//-------------------------------------------------------------//
case '+':
if(objServo[numServo].attached() && cenAngle[numServo]<180){
if(numServo/2%2){
cenAngle[numServo]--;
}
else{
if(numServo%2){
cenAngle[numServo]--;
}
else{
cenAngle[numServo]++;
}
}
}
f=1;
break;
//-------------------------------------------------------------//
case '-':
if(objServo[numServo].attached() && cenAngle[numServo]>0 ){
if(numServo/2%2){
cenAngle[numServo]++;
}
else{
if(numServo%2){
cenAngle[numServo]++;
}
else{
cenAngle[numServo]--;
}
}
}
f=1;
break;
//-------------------------------------------------------------//
case '0': i*=10; i+=0; f=2; break;
//-------------------------------------------------------------//
case '1': i*=10; i+=1; f=2; break;
//-------------------------------------------------------------//
case '2': i*=10; i+=2; f=2; break;
//-------------------------------------------------------------//
case '3': i*=10; i+=3; f=2; break;
//-------------------------------------------------------------//
case '4': i*=10; i+=4; f=2; break;
//-------------------------------------------------------------//
case '5': i*=10; i+=5; f=2; break;
//-------------------------------------------------------------//
case '6': i*=10; i+=6; f=2; break;
//-------------------------------------------------------------//
case '7': i*=10; i+=7; f=2; break;
//-------------------------------------------------------------//
case '8': i*=10; i+=8; f=2; break;
//-------------------------------------------------------------//
case '9': i*=10; i+=9; f=2; break;
//-------------------------------------------------------------//
case '*': f=3; break;
//-------------------------------------------------------------//
case '?': f=4; break;}
//-------------------------------------------------------------//
delay(10);
}
if(f==1){
objServo[numServo].write(cenAngle[numServo]);
Serial.print((String) "Сустав N "+numServo+", угол = "+cenAngle[numServo]+"°");
if(cenAngle[numServo]<10){
Serial.print(" ");
}
else if(cenAngle[numServo]<100){
Serial.print(" ");
}
Serial.print(F(" РЕЗУЛЬТАТ: const int cenAngle[8]={"));
for(f=0; f<8; f++){
Serial.print(cenAngle[f]);
if(f<7){Serial.print(",");}
}
Serial.println("};");
}
if(f==2){
if(i<8){
numServo=i;
f=i%2;
if(!objServo[numServo].attached()){
objServo[numServo].attach(pinServo[numServo]);
}
objServo[numServo].write(cenAngle[numServo]);
Serial.println(F("==========================================================="));
Serial.println((String) "УСТАНОВКА СУСТАВА № "+numServo+": ("+(f?"вертикальный":"горизонтальный")+" сустав "+(numServo/2+1)+" конечности)");
Serial.println(F("==========================================================="));}}
if(f==3){
for(f=0; f<8; f++){
objServo[f].detach();
digitalWrite(pinServo[f],LOW);
}
Serial.println(F("===========================================================\r\nВСЕ СУСТАВЫ ОСВОБОЖДЕНЫ!\r\n==========================================================="));}
if(f==4){
Serial.println(F("===========================================================\r\nВведите № сустава от 0 до 7 включительно (заработает его сервопривод)\r\nВводя знаки + или - установите сустав в центральное положение\r\nКаждый знак + или - соответствует сдвигу сустава на 1°\r\nНапример, если ввести ++++++ то сустав сдвинется на 6°\r\nПосле каждого сдвига высвечивается угол сустава и общий результат\r\nКогда Вы отцентруете положение всех суставов, вставьте последний\r\nполученный общий результат вместо 4 строки основного скетча\r\n==========================================================="));}
}
}
Примечание:
Калибровочный скетч выводит информацию на Русском языке, что доступно только в версиях Arduino IDE 1.8.2 и выше!
Обсуждение