8 (800) 350-30-16

MePed. Алгоритмы и программирование. Часть 2

Дата: / Категория: Механика / Автор: DOC_tr

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

Для начала я купить блок питания RS-50-5, который выдает 5v/10a. По факту, как выяснилось он может выдавать до 6v (если выкрутить потенциометр, или что там в БП находится на максимум), что было очень кстати, потому что MG90S максимальное усилие в 2.2 кг он выдает как раз при 6v.

Кстати, на заметку - MG90 для данного робота лучше чем MG90S, так как MG90S быстрее, но второй сильнее - 2.2 против 2.5 кг/см

Вот кстати сам блок

По сравнению с 12v на 3А он выглядит гигантом - раза в 2 больше.

Теперь, когда у меня появилась нормальный источник питания я могу продолжать свои эксперименты

Робот больше не глючит, не складывается и не бьется в агониях, и мощности хватает, помимо всех приводов, еще и на bluetooth модуль hc-05.

Как вы можете видеть - конструкция с прошлого раза не изменилась. Разве только что теперь я подвожу питание напрямую к ногам на плате. Так как помимо сервов к шилду подключен только bluetooth, который легко справляется с напряжением до 6v, проблем никаких не возникло.

На фото видим что все по прежнему достаточно сумбурно, но главное, что все работает.

Перейдем непосредственно к настройке нашего робота.

Наша схема сборки выглядит следующим образом

Для начала расскажу о его настройке.

Первым делом нам нужно определить угол самой высокой позиции двигателей ног и самой низкой (в скетче строки 33-38. Сам скетч большой, и выложу в конце статьи)

Сделать это можно с помощью скетча из данной статьи, с помощью которого, через консоль, мы задаем позицию двигателя.

int 
aL[2] = {135, 55},
bL[2] = {115, 35},
cL[2] = {130, 50},
dL[2] = {130, 50}
;

Как вы видите - первая буква переменной указывает на ногу (см. схему), а сама переменная представляет из себя массив из 2х чисел (наивысшая и наинизшая позиции ног). Разница между ними должна быть 80 градусов, а второе число массива должно быть меньше первого.

Это первый блок, который нам нужно изменить.

И второй блок, это положение ног (строки 21-26)

int 
aK = 45, // не больше 45
bK = 150, // не меньше 125
cK = 30, // не больше 45
dK = 135 // не меньше 125
;

Нам нужно узнать, при каких углах ноги будут параллельны друг другу. Как на данном фото

При чем, их значения не должны выходить за те, которые указаны в комментариях к ним. Если значения выходят за рамки, то стоит переставить коромысло двигателя.

Именно поэтому лучше всего закреплять коромысла у двигателей только после настройки.

Все остальные необходимые расчеты скетч проведет за вас

На этом все - с роботом разобрались. После данных изменений, если в loop написать forvard();, back(); то робот будет не управляемо двигаться в одну из сторон. 

Теперь нам нужно будет научиться им управлять. В прошлый раз я делал управлял манипулятором с помощью приложения Arduino Bluetooth Controller, но сейчас я нашел более удачное приложение, как по части интерфейса, так и по части передачи данных - DK Gadjet. Подробно о них я напишу в следующей статье, а в этой расскажу вкратце.

Задаем команды с помощью следующих строк

// Команды от мобильного
Command inputCommands[] = {
Command(1, "j", "h", false),
Command(2, "z", "x", false),
};
DKParser parser(inputCommands, 2);

Первая команда - джойстик, вторая - скорость.

Первый аргумент - номер команды, второй и третий - команда начала и команда окончания. Чтобы было ясно - вот пример добавления джойстика через приложение

После добавления джойстика и ползунка, уже можно управлять роботом через bluetooth.

Как видите - это не сильно сложно, а теперь перейдем к скетчу. Все неочевидные моменты я прокомментировал в коде, так что вопросов быть не должно.

Не смотря на то, что сам скетч такой большой и не совсем понятный, если вы все сделали как я написал выше, проблем быть никаких не должно.

#include <Servo.h>
#include <DKParser.h>
Servo servo[8];

// Команды от мобильного
Command inputCommands[] = {
Command(1, "j", "h", false),
Command(2, "z", "x", false),
};
DKParser parser(inputCommands, 2);

int a[4], b[4], c[4], d[4];
int serv_location[8];

/*
* ==========================================================
* Позиции ног при параллельном расположении
* Требуется изменить
* ==========================================================
*/
int
aK = 45, // не больше 45
bK = 150, // не меньше 125
cK = 30, // не больше 45
dK = 135 // не меньше 125
;
/*
* ==========================================================
* Высота ног - максимальное и минимальное значения
* Требуется изменить
* ==========================================================
*/
int
aL[2] = {135, 55},
bL[2] = {115, 35},
cL[2] = {130, 50},
dL[2] = {130, 50}
;

int spd = 15; // скорость
int height = 40; // высота ног

void setup(){
// Базовые настройки - привязка двигателей к пинам, выставление позиций ног и т.п.
servo[0].attach(11);
servo[1].attach(9);
servo[2].attach(5);
servo[3].attach(3);
int koeff = 35;
aK += koeff;
bK -= koeff;
cK += koeff;
dK -= koeff;
for(int i = 0; i < 4 ; i++){
a[i] = aK;
b[i] = bK;
c[i] = cK;
d[i] = dK;
aK += 30;
bK -= 30;
cK += 30;
dK -= 30;
}
servo[0].write(a[1]);
servo[1].write(b[1]);
servo[2].write(c[1]);
servo[3].write(d[1]);
serv_location[0] = a[1];
serv_location[1] = b[1];
serv_location[2] = c[1];
serv_location[3] = d[1];

servo[4].attach(12);
servo[5].attach(10);
servo[6].attach(6);
servo[7].attach(4);
servo[4].write(arf(aL));
servo[5].write(arf(bL));
servo[6].write(arf(cL));
servo[7].write(arf(dL));
serv_location[4] = arf(aL);
serv_location[5] = arf(bL);
serv_location[6] = arf(cL);
serv_location[7] = arf(dL);
Serial.begin(9600);
}

// Текущая позиция каждого шага
int position[4] = {0, 0, 0, 0};
// коэффициенты изменения позиции ног для движения вперед и назад
int koeff[2][4] = {
{-12, 30, -18, 12},
{-24, 12, 12, 36}
};
int move = 0;
void loop(){
// Двигаемся в зависимости от запомненного положения джойстика.
switch(move){
case 1:
forvard();
break;
case 2:
left();
break;
case 3:
back();
break;
case 4:
right();
break;
}
// Считываем положение джойстика
Command command = parser.commandAvailable();
switch (command.getId()) {
case 1:{
// Данные приходят вида ([0-9]+),([0-9]+)
// Первое число - градус, второе - удаление джойстика от центра (100 у самого края, 0 джойстик не двигается)
int angle = getValue(command.getValue(), ',', 0).toInt();
int sped_j = getValue(command.getValue(), ',', 1).toInt();
// если джойстик не двигается, то не идем вне зависимости от угла
if(sped_j <= 10){
move = 0;
break;
}
if(angle >=45 && angle < 135){ // вперед
move = 1;
} if(angle >= 135 && angle < 225){ // влево
move = 2;
} if(angle >= 225 && angle < 315){ // назад
move = 3;
} if(angle >= 315 || angle < 45){ // вправо
move = 4;
}
break;
}case 2:{
// устанавливаем скорость в зависимости от ползунка
spd = map(command.getValue().toInt(), 0, 100, 30, 3);
break;
}
}
}

// шаг влево.
// В массиве положение ног.
void left(){
int data[8][12] = {
{a[2], b[0], c[0], d[2], arf(aL), arf(bL) + 30, arf(cL), arf(dL), 1, 3, 1, 1},
{a[2], b[0], c[0], d[2], arf(aL), arf(bL), arf(cL), arf(dL), 1, 3, 1, 1},
{a[1], b[1], c[3], d[3], arf(aL), arf(bL), arf(cL) + 30, arf(dL), 1, 1, 3, 1},
{a[1], b[1], c[3], d[3], arf(aL), arf(bL), arf(cL), arf(dL), 1, 1, 3, 1},
{a[0], b[2], c[2], d[0], arf(aL), arf(bL), arf(cL), arf(dL) + 30, 1, 1, 1, 3},
{a[0], b[2], c[2], d[0], arf(aL), arf(bL), arf(cL), arf(dL), 1, 1, 1, 3},
{a[3], b[3], c[1], d[1], arf(aL) + 30, arf(bL), arf(cL), arf(dL), 3, 1, 1, 1},
{a[3], b[3], c[1], d[1], arf(aL), arf(bL), arf(cL), arf(dL), 3, 1, 1, 1}
};

moves(
data[position[2]][0],
data[position[2]][1],
data[position[2]][2],
data[position[2]][3],
data[position[2]][4],
data[position[2]][5],
data[position[2]][6],
data[position[2]][7],
data[position[2]][8],
data[position[2]][9],
data[position[2]][10],
data[position[2]][11]
);
position[2] ++;
if(position[2] >= 8){
position[2] = 0;
}
}


void right(){
int data[8][12] = {
{a[0], b[2], c[2], d[0], arf(aL) + 30, arf(bL), arf(cL), arf(dL), 3, 1, 1, 1},
{a[0], b[2], c[2], d[0], arf(aL), arf(bL), arf(cL), arf(dL), 3, 1, 1, 1},
{a[1], b[1], c[3], d[3], arf(aL), arf(bL), arf(cL), arf(dL) + 30, 1, 1, 1, 3},
{a[1], b[1], c[3], d[3], arf(aL), arf(bL), arf(cL), arf(dL), 1, 1, 1, 3},
{a[2], b[0], c[0], d[2], arf(aL), arf(bL), arf(cL) + 30, arf(dL), 1, 1, 3, 1},
{a[2], b[0], c[0], d[2], arf(aL), arf(bL), arf(cL), arf(dL), 1, 1, 3, 1},
{a[3], b[3], c[1], d[1], arf(aL), arf(bL) + 30, arf(cL), arf(dL), 1, 3, 1, 1},
{a[3], b[3], c[1], d[1], arf(aL), arf(bL), arf(cL), arf(dL), 1, 3, 1, 1}
};

moves(
data[position[3]][0],
data[position[3]][1],
data[position[3]][2],
data[position[3]][3],
data[position[3]][4],
data[position[3]][5],
data[position[3]][6],
data[position[3]][7],
data[position[3]][8],
data[position[3]][9],
data[position[3]][10],
data[position[3]][11]
);
position[3] ++;
if(position[3] >= 8){
position[3] = 0;
}
}

void forvard(){
int data[8][12] = {
{a[3] + koeff[0][0], b[3] + koeff[0][1], c[1] + koeff[0][2], d[1] + koeff[0][3], arf(aL), arf(bL), arf(cL), arf(dL), 1, 3, 1, 1},
{a[0] + koeff[0][0], b[2] + koeff[0][1], c[0] + koeff[0][2], d[2] + koeff[0][3], arf(aL) + 30, arf(bL), arf(cL), arf(dL), 3, 1, 1, 1},
{a[0] + koeff[0][0], b[2] + koeff[0][1], c[0] + koeff[0][2], d[2] + koeff[0][3], arf(aL), arf(bL), arf(cL), arf(dL), 3, 1, 1, 1},
{a[1] + koeff[0][0], b[1] + koeff[0][1], c[3] + koeff[0][2], d[3] + koeff[0][3], arf(aL), arf(bL), arf(cL) + 30, arf(dL), 1, 1, 3, 1},
{a[1] + koeff[0][0], b[1] + koeff[0][1], c[3] + koeff[0][2], d[3] + koeff[0][3], arf(aL), arf(bL), arf(cL), arf(dL), 1, 1, 3, 1},
{a[2] + koeff[0][0], b[0] + koeff[0][1], c[2] + koeff[0][2], d[0] + koeff[0][3], arf(aL), arf(bL), arf(cL), arf(dL) + 30, 1, 1, 1, 3},
{a[2] + koeff[0][0], b[0] + koeff[0][1], c[2] + koeff[0][2], d[0] + koeff[0][3], arf(aL), arf(bL), arf(cL), arf(dL), 1, 1, 1, 3},
{a[3] + koeff[0][0], b[3] + koeff[0][1], c[1] + koeff[0][2], d[1] + koeff[0][3], arf(aL), arf(bL) + 30, arf(cL), arf(dL), 1, 3, 1, 1}
};

moves(
data[position[0]][0],
data[position[0]][1],
data[position[0]][2],
data[position[0]][3],
data[position[0]][4],
data[position[0]][5],
data[position[0]][6],
data[position[0]][7],
data[position[0]][8],
data[position[0]][9],
data[position[0]][10],
data[position[0]][11]
);
position[0] ++;
if(position[0] >= 8){
position[0] = 0;
}
}

void back(){
int data[8][12] = {
{a[3] + koeff[1][0], b[3] + koeff[1][1], c[1] + koeff[1][2], d[1] + koeff[1][3], arf(aL), arf(bL), arf(cL), arf(dL), 3, 1, 1, 1},
{a[2] + koeff[1][0], b[0] + koeff[1][1], c[2] + koeff[1][2], d[0] + koeff[1][3], arf(aL), arf(bL) + 30, arf(cL), arf(dL), 1, 3, 1, 1},
{a[2] + koeff[1][0], b[0] + koeff[1][1], c[2] + koeff[1][2], d[0] + koeff[1][3], arf(aL), arf(bL), arf(cL), arf(dL), 1, 3, 1, 1},
{a[1] + koeff[1][0], b[1] + koeff[1][1], c[3] + koeff[1][2], d[3] + koeff[1][3], arf(aL), arf(bL), arf(cL), arf(dL) + 30, 1, 1, 1, 3},
{a[1] + koeff[1][0], b[1] + koeff[1][1], c[3] + koeff[1][2], d[3] + koeff[1][3], arf(aL), arf(bL), arf(cL), arf(dL), 1, 1, 1, 3},
{a[0] + koeff[1][0], b[2] + koeff[1][1], c[0] + koeff[1][2], d[2] + koeff[1][3], arf(aL), arf(bL), arf(cL) + 30, arf(dL), 1, 1, 3, 1},
{a[0] + koeff[1][0], b[2] + koeff[1][1], c[0] + koeff[1][2], d[2] + koeff[1][3], arf(aL), arf(bL), arf(cL), arf(dL), 1, 1, 3, 1},
{a[3] + koeff[1][0], b[3] + koeff[1][1], c[1] + koeff[1][2], d[1] + koeff[1][3], arf(aL) + 30, arf(bL), arf(cL), arf(dL), 3, 1, 1, 1}
};

moves(
data[position[1]][0],
data[position[1]][1],
data[position[1]][2],
data[position[1]][3],
data[position[1]][4],
data[position[1]][5],
data[position[1]][6],
data[position[1]][7],
data[position[1]][8],
data[position[1]][9],
data[position[1]][10],
data[position[1]][11]
);
position[1] ++;
if(position[1] >= 8){
position[1] = 0;
}
}

// двигаем всю конструкцию
void moves(int keyA, int keyB, int keyC, int keyD, int keyLA, int keyLB, int keyLC, int keyLD, int sA, int sB, int sC, int sD){
while(
serv_location[0] != keyA ||
serv_location[1] != keyB ||
serv_location[2] != keyC ||
serv_location[3] != keyD ||
serv_location[4] != keyLA ||
serv_location[5] != keyLB ||
serv_location[6] != keyLC ||
serv_location[7] != keyLD
){
if(keyA > serv_location[0])serv_location[0]+=sA;
if(keyA < serv_location[0])serv_location[0]-=sA;
if(keyB > serv_location[1])serv_location[1]+=sB;
if(keyB < serv_location[1])serv_location[1]-=sB;
if(keyC > serv_location[2])serv_location[2]+=sC;
if(keyC < serv_location[2])serv_location[2]-=sC;
if(keyD > serv_location[3])serv_location[3]+=sD;
if(keyD < serv_location[3])serv_location[3]-=sD;

if(keyLA > serv_location[4])serv_location[4]+=sA;
if(keyLA < serv_location[4])serv_location[4]-=sA;
if(keyLB > serv_location[5])serv_location[5]+=sB;
if(keyLB < serv_location[5])serv_location[5]-=sB;
if(keyLC > serv_location[6])serv_location[6]+=sC;
if(keyLC < serv_location[6])serv_location[6]-=sC;
if(keyLD > serv_location[7])serv_location[7]+=sD;
if(keyLD < serv_location[7])serv_location[7]-=sD;

for(int i = 0 ; i < 8 ; i++){
servo[i].write(serv_location[i]);
}
delay(spd);
}
}

// высота ног
int arf(int h[2]){
return h[1] + height;
}

// split
String getValue(String data, char separator, int index){
int found = 0;
int strIndex[] = {0, -1};
int maxIndex = data.length()-1;
for(int i=0; i<=maxIndex && found<=index; i++){
if(data.charAt(i)==separator || i==maxIndex){
found++;
strIndex[0] = strIndex[1]+1;
strIndex[1] = (i == maxIndex) ? i+1 : i;
}
}
return found>index ? data.substring(strIndex[0], strIndex[1]) : "";
}

Видео управление данным роботом через bluetooth.

Теги: #Робот, #MePed, #Алгоритмы

Ваша оценка:

Рейтинг: 9.8 (Оценок: 3)

Комментарий:

Copyright © RobOM.ru 2016 г. Все права защищены