Дорисуй картинку
У меня столько вопросов, что там происходит
У меня столько вопросов, что там происходит
Предыстория (для тех кто не в кугсе, часть -1) : решили мы с коллегой пособирать что либо на ардуино, его выбор пал на шестинога (чаще встречается название гексапод, паук и т.д.).
Вступление. В этой части рассмотрим геометрию хождения «паучьих», терминологию (сразу оговорюсь я ее применять не буду, но для академического интереса опишу). Вспомним (для кого-то подтянем, может даже и для самих себя после каментов) тригонометрию, опишем ее, переложим в код и проверим все это в работе одной ноги.
Железо по прежнему 1 arduino nano, 18 сервоприводов tower pro MG996R (точнее их дешевые клоны-уже писал что это никак не сопоставимо с оригиналом, но дешево) 4 аккумулятора 18650 2х2 (7.4В 4.8 Ач) несколько преобразователей напряжения. И шилд расширения шим выводов или еще одна arduino nano (не решено, но опробовано и так и так).
Обещанная терминология лап.
Чаще применяются 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 мощных привода нужно запитывать уже от отдельного источника.
Статья вступление, здесь не будет количества деталей (легкое описание), подробного описания подхода к сборке, а только «как все начиналось».
Побудило к написанию отсутствие адекватных описаний "на пальцах" как это все создается .
Коллега на работе захотел что-либо построить для изучения arduino, к тому времени у меня накопилось много идей и некоторые я научился воплощать. Не желая погружаться в проблему целиком решил дать ему «..одно маленькое, но очень отвессвенное поручение..»
задание с которым справится только желающий этим заниматься, я дал ему FreeCAD, показал как рисовать и сказал искать платформу и перерисовывать «под себя»!
Надо сказать он справился с этой задачей! Мы взяли платформу, он ее немного уменьшил, подогнал под, тогда уже накупленные 6 сервоприводов, отверстия чертеж (на фото не мое, но похоже).
Остов и ноги мы вырезали на лазерном станке из фанеры 4мм, крепления и ответные втулки вращения корневых серв напечатали на 3D Принтере. К тому моменту я докупил еще приводов до полного комплекта чтобы подстегнуть «подмастерье». Сервы были выбраны не самые лучшие, но самые недорогие и прочные. Это китайские подделки копии (по механике и габаритам) TowerPRO MG946 (MG995). 12 кг усилие по паспорту. Опробовав разные версии этих серв пришел к выводу что годными являются те у которых латунный выход на крепление, а не металлического оттенка
(у оригинала латунный).
Внутри может быть не как у оригинала, но они мне понравились. Не питайте мокрых надежд, конечно они не цифровые (если куплены по минимальной цене) скорость отклика и работы ниже, но жажда творчества это для меня окупает.
Платформу мы взяли arduino nano (full china) добавим плату расширения на 16 каналов или запилим вторую arduino nano по I2C (может оба варианта покажем)
Платформа на 70% готова, основные движения отдельных ног отработаны. В следующей статье будет описание геометрии (тригонометрии), количества требуемых деталей, даже уже вес (взвешенного вживую робота).
П.С. хотел назвать свою статью : -по стопам «Пробная платформа с шестью ножками..» но это не так, хотя приятно увидеть что кто то тоже хочет разобраться во всем с нуля!
П.П.С. Поражает обилие конструкций в сети и либо никакого описания процесса, либо выкладывают совершенно дикие способы ( по мне так доставание гланд через *) с кучей библиотек в кокой нибудь системе 3-х мерного проектирования которую написал "акимото" и т.д.
П.П.П.С. Это не очередной рассказ о том как наша галактика прекрасна и в ней много красивых роботов, взгляните на моего! Все чертежи, скетчи и прочее необходимое для повторение будет выкладываться.