Чаще применяются 3 части Coxa, Femur, Tribia. Я назову Плечо, Предплечье и Лапа.

Про код. Чаще встречаешь в сети из легко доступного код который может совершить конкретные действия в конкретном механизме заранее известного размера, что весьма печалит ибо всегда хочется совершать и свои движения. Постараемся написать именно такую функцию которая может передвигать лапу в любом направлении по «рисунку» на координатной плоскости (по сути по описанию промежуточных точек, в массиве, с координатами X Y Z)

Про термины. Нам понадобятся функции sin(A), cos(A), tan(A).

sin(A) отношение противолежащего (относительно угла) катета к гипотенузе

cos(A) отношение прилежащего (относительно угла) катета к гипотенузе

tan(A) отношение противолежащего катета к прилежащему или синуса к косинусу угла.

Еще нам понадобится формула отыскания угла в непрямоугольном треугольнике .

Про переменные. Переменные которые мы заведем (что то будет заведено не переменными а "задефайнено"=#define эта штука запоминает первую часть описания и меняет на вторую во всем коде, не занимает место в оличии от переменных и в нее удобно поместить вещи которые не нужно менять, наши габаритные параметры лап). Первые из переменных это координаты в пространстве x, y, z, по ним зададим точки в пространстве по которым будем перемещать лапы.

Углы которые будем считать q_0 угол плеча, q_1, q_2-угол сервопривода лапы, A-угол сервопривода предплечья, F.

Еще величины использующиеся при расчете r_1 длина предплечья, r_2 длина лапы, vinos_r_1 расстояние от оси Z (оси поворота плеча, корневой сервы) до оси поворота предплечья (второго сервопривода). Дело в том что эта величина всегда перпендикулярна оси Z и она не участвует в геометрии треугольника, не учесть мы ее не можем, так как точки 1 и 2 сервопривода (плечо и предплечье) нельзя совместить в одной точке, но учесть ее оказалось не сложно.

Вот так я для себя отобразил углы и стороны треугольника сопоставив с размерами между осями сервоприводов

Для чего я это выписал величины в начале, первое чтобы изучить то на чем мы будем строить расчет нашего движения, второе чтобы заранее подумать как мы это будем «засовывать» в ардуино. Функции синус и косинус в ардуино есть и тут ничего сверх сложного нет, но нужно понимать какого порядка будут величины, в чем их можно хранить (имеется введу типы переменных) а где они могут потеряться если их присвоить в другой тип данных. Еще один нюанс, нельзя забывать что расчет всех величин как в эксель так и в ардуино ведется в радианах. Полученные углы нужно будет переводить в градусы.

Пишем код. Все что нам нужно определить заранее (известные величины: длина предплечья, лапы, вынос) выставим это все в начале скетча

#define PI 3.1416

#define rad2deg(x) ((180.0/PI)*(x))// перевод в градусы

#define r_1 56 //мм

#define r_2 90 //мм

#define vinos_r_1 30 //мм

#define Plecho_1_pin 3

#define PredPlecho_1_pin 4

#define Lapa_1_pin 5

Заведем перменные

float x_mass[]={-40,-30,-20,-10,0,10,20,30,40}; //перемещение вперед, назад координата X

float z_mass[]={-15,-25,-40,-65,-75}; //перемещение вверх, вниз назад координата Z

float y_mass[]={90,90,90,90,90,90,90}; //расстояние от оси поворота плеча до точки касания лапой земли не будем пока что менять

Инициализируем приводы

Servo Lapa1;

Servo Plecho1;

Servo PredPlecho1;

Заводим первую функцию сетап

void setup() {

Plecho1.attach(Plecho_1_pin ); // к ардуино 3

PredPlecho1.attach(PredPlecho_1_pin); // к ардуино 4

Lapa1.attach(Lapa_1_pin); // к ардуино 5

}

void loop () {

raschet_uglov (); //функция расчета углов

ugol_per_prav (); //функция вывода углов в сервоприводы

delay(1000); //для тестов поставим задержку чтобы лихорадочно не дергаться

}

Посмотрим теорию по расчету углов треугольника.

Смотрим сюда Вики и на картинки

Для проверки формул (вживую не хочется жечь сервы, когда они зависают в крайних углах) я завел таблицу excel и вбил туда все эти формулы для проверки возможности перемещения ноги в заданные координаты. Ее прикладывать не буду (не жалко, но кто хочет сами потренируйтесь)

Теперь пишем сами функции расчета углов и вывода их в сервоприводы.

void raschet_uglov ()

{q_0 = atan((x_mass[0])/( y_mass[0])); // угол плеча

float L = sqrt(sq(x_mass[0])+sq(y_mass[0]));

float _V_ = L-vinos_r_1;

float _L = sqrt(sq(_V_)+sq(z_mass[0]));

float q_2 = acos(z_mass[0]/_L);

float r_1sq=sq(r_1);

float r_2sq=sq(r_2);

float _Lsq=sq(_L);

F = (r_1sq+r_2sq-_Lsq)/(2*r_1*r_2); // расчет угла смотрим вики, расчет углов треугольников

q_1 = (r_1sq-r_2sq+_Lsq)/(2*r_1*_L); // смотрим вики, расчет углов треугольников

F = acos(F); // угол лапа

q_1 = acos(q_1);

A = (rad2deg(q_2)-rad2deg(q_1));} // предплечье

Теперь выводим эти углы в сервоприводы

void ugol_per_lev ()

{ Plecho1.write(90+rad2deg(q_0));

PredPlecho1.write (A);

Lapa1.write(rad2deg(F));}

На этом все. Двигать лапами мы в этот раз не будем никакими воздействиями, думаю и так уже дым из ушей, просто заняли положение крайнее вперед из наших матриц. Если ходите попередвигать то просто перебирайте координатой х (опять таки из массива) настоящее движение мы будем писать в следующей статье…

П.С. Прямо этот текст не проверял, но все рабочее 100% (просто выдернуто из общего скетча) пишите в комментариях если что не так.

П.П.С. В следующей части будет железо и подключение всего что связано с лапами. Чтобы не было дыма из ардуино, 3 мощных привода нужно запитывать уже от отдельного источника.