Хотел бы разобрать код открытого проекта робота-"паука".

Самому проекту уже более 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;
}

Комментарии и названия переменных мало что дают. Лично я убил несколько дней на разбор этого участка кода.

По шагам:

1. Рассчитывается длина вектора w (от центра координат до целевой точки в плоскости XY)

2. "Виртуально" перемещаемся в плоскость, задаваемую вектором w и осью Z.

3. На эту плоскость проецируется положение костей

4. Смещаем центр координат в соединение coxa-femur

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

И становится понятно назначение переменных:

• alpha - угол между coxa и femur

• beta - угол между femur и tibia

• gamma - угол поворота coxa

P.S.:

Разобравшись, как что то работает можно изменять и/или создавать свое на похожих принципах. Если подумать, то у тех же роботов собак получается, что перемещение лап сводится к тому же треугольнику с тремя известными сторонами....

В разборе кода очень помогла статья "Инверсная кинематика в 2D"