Хотел бы разобрать код открытого проекта робота-"паука".
Самому проекту уже более 10-ти лет, но лично мне интересен тем, что в отличии от других подобных проектов использует математику для описания движений, а не захардкоженные последовательности из углов поворота сервоприводов.
Первое знакомство и основные характеристики
Есть несколько версий данного робота.
В частности я наткнулся на одну из модификаций, ища что бы такого напечатать на свежекупленном 3D-принтере.
Основной репозиторий исходного проекта располагается на GitHub автора, но там нет некоторых деталей. В частности там нет кода, отвечающего за выравнивание сервоприводов перед затяжкой винтов.
Инструкции по сборке и первоначальной настройке можно найти по ссылке: http://www.instructables.com/id/DIY-Spider-RobotQuad-robot-Quadruped/ (есть проблемы с отображением страницы - CDN попали под блокировки, копия здесь)
Характеристики робота:
4 "ноги"
Каждая "нога" состоит из трех сервоприводов типа SG90/MG90
В качестве "мозга" - Arduino Nano(но можно использовать любую совместимую с Arduino плату)
В качестве дополнений существует версии кода с удаленным управлением по Bluetooth.
Так как сами сервоприводы в пике могут потреблять значительную мощность, то крайне желательно использовать мощный DC-DC преобразователь и контроллер заряда аккумуляторов.
Я же на свой страх и риск использую готовую плату, которая может и не выдержать такого надругательства.
Скелеты и кости
Можно сказать что робот симметричен относительно своей оси.
Поэтому можно рассмотреть только одну "лапу":
coxa - "сустав", вращает ногу вокруг оси Z, тем самым задает плоскость, в которой может перемещаться остальная часть
femur - средняя "кость"
tibia - опорная кость, в некоторых роботах отсутствует
Начальное положение костей относительно нейтральных позиций сервоприводов:
Разгребая код
Структурно код выглядит стандартно для большинства "скетчей" - масса глобальных переменных, функции setup и loop, и т.д.
Из нестандартных библиотек используется FlexiTimer2 и Servo
Блок переменных и констант требует дополнительных комментариев:
//define servos' ports /// 4 ноги по три сервы /// Тут ВНИМАНИЕ: каждая нога апределена в поряде: {femur, tibia, coxa} /// Что несколько противоречит ожидаемому {coxa, femur, tibia} /// И это нужно учитывать при подключении серв к плате const int servo_pin[4][3] = { {2, 3, 4}, {5, 6, 7}, {8, 9, 10}, {11, 12, 13} }; /* Size of the robot ---------------------------------------------------------*/ // Длина femur const float length_a = 55; // Длина tibia const float length_b = 77.5; // Длина coxa const float length_c = 27.5; // ??? const float length_side = 71; // Смещение низа корпуса от оси соединения coxa-femur const float z_absolute = -28; /* Constants for movement ----------------------------------------------------*/ const float z_default = -50, z_up = -30, z_boot = z_absolute; const float x_default = 62, x_offset = 0; const float y_start = 0, y_step = 40; const float y_default = x_default; /* variables for movement ----------------------------------------------------*/ // Текущее РАССЧИТАННОЕ положение volatile float site_now[4][3]; //real-time coordinates of the end of each leg // Текущее заданное положение volatile float site_expect[4][3]; //expected coordinates of the end of each leg // дельты, на которые сервы смещаются каждый тик float temp_speed[4][3]; //each axis' speed, needs to be recalculated before each movement
Основные функции, отвечающие за передвижение робота:
set_site- установка целевого положения опоры для конкретной "лапы", расчет скоростейwait_all_reach- ожидание достижения установленных точек - в цикле сравнивает текущее и заданное положениеservo_service- наиболее интересная функция, отвечает за пересчет координат в углы поворота сервоприводов
В set_site есть баг: при дельте перемещения равной нулю в скорости получается Infinity. Но "стреляет он только на платформах с аппаратной float-математики. На ATMega этого нет и используется программная реализация и вероятно она чем то отличается от поведения аппаратной. Меня это затронуло, когда переносил код на SBC под управлением Linux.
Самое вкусное
Самое интересное находится в функции servo_service, вызываемой с частотой 50Hz
#define E_DELTA 0.01 void servo_service(void) { sei(); static float alpha, beta, gamma; // для каждой ноги for (int i = 0; i < 4; i++) { // для каждй сервы for (int j = 0; j < 3; j++) { // определить следующее положение if (abs(site_now[i][j] - site_expect[i][j]) < (abs(temp_speed[i][j])+E_DELTA)) site_now[i][j] = site_expect[i][j]; else site_now[i][j] += temp_speed[i][j]; } // перевести декартовы координаты в углы поворота cartesian_to_polar(alpha, beta, gamma, site_now[i][0], site_now[i][1], site_now[i][2]); // установить углы серв, скорректировав их в зависимости от номера ноги polar_to_servo(i, alpha, beta, gamma); } rest_counter++; }
А точнее в cartesian_to_polar:
void cartesian_to_polar(volatile float &alpha, volatile float &beta, volatile float &gamma, volatile float x, volatile float y, volatile float z) { //calculate w-z degree float v, w; w = (x >= 0 ? 1 : -1) * (sqrt(pow(x, 2) + pow(y, 2))); v = w - length_c; alpha = atan2(z, v) + acos((pow(length_a, 2) - pow(length_b, 2) + pow(v, 2) + pow(z, 2)) / 2 / length_a / sqrt(pow(v, 2) + pow(z, 2))); beta = acos((pow(length_a, 2) + pow(length_b, 2) - pow(v, 2) - pow(z, 2)) / 2 / length_a / length_b); //calculate x-y-z degree gamma = (w >= 0) ? atan2(y, x) : atan2(-y, -x); //trans degree pi->180 alpha = alpha / pi * 180; beta = beta / pi * 180; gamma = gamma / pi * 180; }
Комментарии и названия переменных мало что дают. Лично я убил несколько дней на разбор этого участка кода.
По шагам:
Рассчитывается длина вектора w (от центра координат до целевой точки в плоскости XY)
"Виртуально" перемещаемся в плоскость, задаваемую вектором w и осью Z.
На эту плоскость проецируется положение костей
Смещаем центр координат в соединение coxa-femur
После чего все сводится к теореме косинусов, часто используемой в инверсной кинематике
И становится понятно назначение переменных:
alpha- угол между coxa и femurbeta- угол между femur и tibiagamma- угол поворота coxa
P.S.:
Разобравшись, как что то работает можно изменять и/или создавать свое на похожих принципах. Если подумать, то у тех же роботов собак получается, что перемещение лап сводится к тому же треугольнику с тремя известными сторонами....
В разборе кода очень помогла статья "Инверсная кинематика в 2D"
