Чаще применяются 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 мощных привода нужно запитывать уже от отдельного источника.
