Знакомьтесь с Петей, шестиногом о трёх сервоприводах.

Продолжаю публикацию статей из серии "ардуино головного мозга". Петя — это очень дешёвый (примерно десять баксов) гексапод. Он может быть прекрасным проектом на один ненастный выходной, который развлечёт как и взрослых, так и детей. Раз уж мы про развлечения, вот вам видеоролик с Петей, танцующим под фанк-музыку:

Разумеется, никакого анализа звука я не делал, просто запрограммировал Петю на танец в определённом ритме. Вот ещё один ролик, в котором Петя выказывает своё презрение к мячикам для жонглирования:

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

Как клонировать Петю

Список покупок

Если у вас есть доступ к 3д принтеру, то распечатать непосредственно тело/ноги робота не будет стоить практически ничего. Вот список основных деталей, необходимых для клонирования Пети:

• Материнская плата. Её вполне можно сделать дома лазерным утюгом, ну или заказать в Китае. Без каких-либо скидок это будет стоить примерно 10€ / 10 шт (включая пересылку), со скидками может спуститься до 2€ / 10 шт. Поскольку платка микромизерная, то я её добавил к другому заказу, и она мне оказалсь вообще бесплатной. Если изготовить плату на заказ — не вариант, то можно найти альтернативу типа cheapduino или чего-то подобного, поскольку схема крайне примитивная.
• SG90 9G сервопривды, 3 * 1.47€ / шт
• 4x AAA держалка для батареек, 1.34€ / шт
• ATMega8A-AU (QFP-32), 1€ / шт
• IR LED + IR фототранзистор, 0.20€ / шт
• Электролитический конденсатор 1000uF 16V, 0.17€ / шт
• 2n3904 транзисторы, 3 * 0.01€ / шт
• А также вам понадобятся провода, термоусадка, винты, гребёнки и несколько 0805 резисторов и конденсаторов. Полный список компонентов для материнки приведён тут: hardware/motherboard/BOM.html.

Покупать конденсаторы штучно, конечно, никто не будет. Лично я конкретно под этот проект заказывал платы (бесплатно, поскольку в нагрузку к другому проекту), три сервопривода и держалку для батареек. Вся остальная мелочёвка у меня лежит в куче всякого другого хлама. Итого десять баксов — это ещё с запасом. Кстати, самой дорогой вещью в этом роботе могут оказаться батарейки ;)

NB: При заказе 9g сервоприводов не забудьте, что они бывают разных размеров. Петя предполагает использвание низкопрофильных серв с пластиковыми редукторами. Конечно, он будет работать и с другими сервами, но в этом случае не исключено, что придётся внести косметические изменения в SketchUp файлы. Ну и вообще металлический редуктор в данном случае не только оверкилл, но и зло в виде излишнего жужжания, трения, энергопотребления и выламывания деталей в случае форсмажора.

Тело

Тут сюрпризов нет, если у вас есть доступ к 3д принтеру, просто напечатайте содержимое каталога hardware/body/. Распечатанные детали выглядят как-то так::

У меня на принтере стоит сопло диаметром 1мм, так что все детали распечатались за пару часов в сумме. После сборки оно должно выглядеть как-то так:

М3 нейлоновые винты идеально подходят для сборки. Используйте нейлоновые шайбочки между движущимися частями, и затормозите гайку на резьбе любым удобным вам способом. Лично я просто нежно ткнул паяльником:

Материнка

Мозги

Сама по себе материнка крайне примитивная. Исходники и гербер лежат в папке hardware/motherboard/. Вот рендер гербера:

Материнка несёт на себе микроконтроллер ATMega8 и схему датчика препятствий, и ничего кроме этого. Вот так выглядит схема подключения мозга:

Я рекомендую сначала запаять строгий минимум, необходимый для запуска процессора, чисто чтобы убедиться, что деликатная пайка в полном порядке. На этом этапе материнка выглядит вот так:

N.B. Обратите внимние, что даташит ATMega8A предписывает рабочее напряжение в диапазоне 2.7-5.5В, и абсолютный максимум напряжения 6В. Безопасный вариант питания Пети — это четыре NiMH 1.2V аккумулятора. Чисто из духа противоречния я перекрестился и засунул в батарейный отсек четыре стандартные щелочные батарейки (6.4В в сумме), и это не сожгло Пете мозги, он вполне нормально бегает. Если вы пойдёте этой дорогой, я вас предупреждал, вы это делаете на свой страх и риск!

Вот фотография полностью распаянной материнской платы (за исключением ИК светодиодов и фототранзисторов):

Датчик препятствий

У Пети два глаза, каждый из них состоит из инфракрасного светодиода и соответствующего фототранзистора. Светодиод излучает инфракрасный свет; этот свет распространяется через воздух и отражается от препятствий назад к фотоприёмнику. Если препятствие близко, отражённый свет будет сильнее, нежели если пепрятствие находится далеко. Обратите внимание, что хоть инфракрасный свет и не виден невооружённым глазом, некоторые камеры его регистрируют и могут показать на записи, что весьма удобно для отладки:

Схема датчика препятствий крайне примитивна:

Мы запитываем два светодиода; когда фототранзисторы не освещены, коллекторы Q3 и Q4 "привязаны" к Vcc, а когда фототранзисторы улавливают достаточное количество ИК излучения, напряжение на коллекторах падает. На следующем примере я тестирую эту схему перед установкой светодиодов и фототранзисторов в глазницы Пети:

Обратите внимание, что в зависимости от того, какие у вас светодиоды, вам может быть нужно подобрать значение резистора R6. 47 Ом дают 55мА через светодиоды, но некоторые светодиоды могут хотеть больше (или меньше). Например, я выпаял безызвестные ИК светодиоды из поломанной детской игрушки, и они прекрасно работают на трёх миллиамперах (910 Ом)!

Я рекомендую сначала собрать схему приёмника на макетке (без светодиодов). Затем возьмите ИК светодиод, запитайте его напрямую от таблетки типа CR2032, и направьте в фототранзистор. (Я ничего не знаю про внутреннее сопротивление таблеток, но в моём [небогатом] опыте я никогда не видел изжаренных светодиодов от подобных манипуляций, поправьте меня, если я ошибаюсь)
Осветив ИК излучением фототранзисторы, убедитесь, что напряжение на коллекторах Q3 и Q4 падает согласно ожиданиям. Убедившись, что фотоприёмник работает корректно, подберите необходимый резистор для пары светодиодв, для того, чтобы получить поведение похожее на то, что я привёл в моём видео.

Обратите внимание, что лучше надеть на светодиоды и на фотоприёмники термоусадку для отсекания паразитных засветок. Ну и кроме того, с термоусадкой они прекрасно садятся в глазницы. При запаивании 2n3904, я бы посоветовал сначала запаивать центральный вывод, и только потом боковые, а то очень легко посадить трудноубираемую соплю припоя между ногами. Лично я нахожу пайку этих транзисторов более трудной, нежели пайку самого микропроцессора, но я плохой монтажник.

Да, если у вас нет осциллографа, это не беда, вполне можно обойтись парой светодиодов, обратите внимание на синие светодиоды на этом видео:

Для более далёких препятствий светодиоды будут тусклее.

Если у вас не получается заставить работать датчик препятствий, ну или если вам просто не понравилась предложенная схема, то существует множество других вариантов:

• Можно использовать isf471 вместо фототранзисторов и всей их обвязки типа резисторов и 2n3904.
• Можно купить измеритель расстояния Sharp GP2Y0A21YK0F:
  
• Ну или крайне примитивный готовый китайский сенсор на базе компаратора LM393:
  

Как работает прошивка

В принципе, Петю можно программировать через среду ардуино, но я её нахожу слишком громоздкой и непонятной для такого простого микроконтроллера, как ATMega8. Давайте разобьём объяснение работы прошивки на четыре части:

• как работает ШИМ-генератор
• как Петя двигает конечностями
• последовательности шагов
• обход препятствий

Шим-сигнал

Хоббийные сервоприводы принимают на вход 50 Гц ШИМ сигнал; 1 мс минимальная длительность импульса (0 градусов положения вала), 2 мс максимальная длительность импульса (90 градусов положения вала). У Пети на борту три сервопривода, два из них заведены на 16 битный таймер (timer1), а третий на восьмибитный таймер (timer2). Если я не ошибаюсь, ардуиновская библиотека Servo.h управляет сервами в режиме софтверного ШИМ, что на мой взгляд расточительно, так что у меня оба таймера тикают в режиме fast PWM.

Сам микроконтроллер тикает на 8 МГц, при этом timer1 работает на частоте 1 МГц (делитель 8).
Регистр ICR1 задаёт значение TOP (20000), таким образом, таймер перезапускается каждые 20 мс, выдавая корректный 50 Гц сигнал. Регистры OCR1A и OCR1B контролируют длительность (в микросекундах) импульсов для левой и правой серв.

А вот с центральной сервой проблема. Она заведена на восьмибитный таймер timer2, а он, к сожалению, не имеет аналога столь удобного ICR1, то есть, частота переполнения счётчика контролируется только через делитель. В таблице делителей нет такого, который позволил бы приблизить 50 Гц с приемлемой точностью, так что вот дикая идея, которая лежит где-то посередине между софтверным и хардверным ШИМ генераторами:

• Мы инициализируем таймер timer2 тикать с делителем 128, таким образом, он переполняется через 4.096 ms = 256 * 128/(8 * 10^6).
• Сразу после переполнения мы выключаем timer2, то есть, это он нам выдаёт только один импульс.
• В момент срабатывания capture interrupt таймера timer1 мы перезаряжаем таймер timer2 (и он сработает только на один импульс).

4 мс это больше 2 мс максимальной длительности импульса, которая нам нужна, и сильно меньше перезаряжающего ритма в 20 мс. Подводя итог, если мы хотим поставить все три сервы в среднее положение (1.5 мс длительность импульса), то мы должны сделать следующее:

OCR1A = 1500;    // left servo
OCR1B = 1500;    // right servo
OCR2  = 1500/16; // center servo

Планировщик движений

Перво-наперво, в коде есть шесть важных констант:

const uint8_t  zero[3] = {45, 50, 40};     // zero position of the servo (degrees)
const uint8_t range[3] = {25, 25, 20};     // the servos are allowed to move in the zero[i] +- range[i] interval

Массив zero[3] хранит значения углов для всех трёх сервоприводов, соответствующих нейтральной позиции (см. левую фотографию чуть ниже). В идеале, эти углы должны были бы быть 45° (середина диапазона серв), но на практике дискретность установки ног на зубчатые валы требует отклонения от идеала 45° для того, чтобы добиться симметричности нейтральной позиции. Затем, range[3] предписывает максимальный разрешённый диапазон движения сервоприводов. Это означает, что сервоприводу с индексом i разрешается двигаться только в диапазоне от zero[i]-range[i] до zero[i]+range[i].

Текущее задание положения сервоприводов (в градусах, 0°-90°) хранится в массиве uint8_t pos[3]. Вызов функции update_servo_timers() обновляет значения регистров ШИМ-генератора согласно заданию. Правая фотография из картинки выше соответствует заданию pos[i]=zero[i]+range[i] для всех трёх индексов i=0,1,2.

В текущей реализации все движения планируются как движения с постоянной скоростью. Для этого у меня заведено четыре вспомогательных массива pos_beg[3], pos_end[3], time_start[3] и duration[3]. Давайте предположим, что я хочу двинуть только левой сервой. Для этого нужно выполнить следующие операции:

• скопировать pos[0] в pos_beg[0], это положение, соответствующее началу движения;
• установить pos_end[0] в желаемое положение (по-прежнему в градусах);
• записать в time_start[0] текущую метку времени (миллисекунды, прошедшие с момента загрузки);
• и, наконец, записать желаемое время движения в duration[0] (в секундах). Таким образом, скорость будет (pos_end[0]-pos_beg[0])/duration[0] градусов/сек.

Затем в бесконечном цикле я вызываю функцию movement_planner(), которая обновляет массив текущего задания положения сервоприводов pos[] согласно плану движения, а затем функцию update_servo_timers(), которая обновляет регистры ШИМ-генератора согласно заданию положения pos[].

Походки

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

• шаг 1: {zero[0]-range[0], zero[1]-range[1], zero[2]+range[2]}
• шаг 2: {zero[0]-range[0], zero[1]-range[1], zero[2]-range[2]}
• шаг 3: {zero[0]+range[0], zero[1]+range[1], zero[2]-range[2]}
• шаг 4: {zero[0]+range[0], zero[1]+range[1], zero[2]+range[2]}

Мы можем записать эту последовательность как 2д массив (четыре тройки целевых позиций):

const int8_t advance_sequence[4][3] = {{-1, -1,  1}, {-1, -1, -1}, { 1,  1, -1}, { 1,  1,  1}};

Этот массив говорит нам, что конечное положение сервопривода i на шаге step равно zero[i] + range[i]*advance_sequence[step][i].
Ну а следующий код позволяет Пете идти вперёд неопределённое время:

    uint8_t step = steps_per_sequence-1; // at the initialization stage the (previous) movement is considered to be complete, thus the next movement will be planned starting from the step 0
    while (1) {
        if (is_movement_finished()) {
            step = (step + 1) % 4; // if previous movement is complete, then perform the next step; this variable loops as 0,1,2,3.
            plan_next_movement(step, advance_sequence); // execute next movement
        }
        movement_planner(); // update the servos position according to the planning
        _delay_ms(1);
    }

Обход препятствий

Давайте вспомним, что наш датчик препятствий выдаёт напряжения, заведённые на каналы 4 и 5 АЦП микропроцессора. Для того, чтобы отсечь высокочастотный шум в измерениях (особенно учитывая количество шума, создаваемого сервами), на каждой итерации главного цикла я обновляю переменные adc_left_eye и adc_right_eye по следующему закону, что даёт мне фильтр низких частот:

        adc_left_eye  = adc_left_eye *.99 + adc_read(5)*.01; // low-pass filter on the ADC readings
        adc_right_eye = adc_right_eye*.99 + adc_read(4)*.01;

Частота отсечки может настраивается или через задержку _delay_ms() внутри главного цикла, или же через коэффициент взвешенной суммы .99 и 1-.99 в вышеприведённом коде.

Наличие препятствия определяется как простое пороговое сравнение:

        uint8_t lobst = adc_left_eye  < distance_threshold; // obstacle on the left?
        uint8_t robst = adc_right_eye < distance_threshold; // obstacle on the right?

Затем в конце каждого шага (напоминаю, четыре шага на каждую последовательность) я проверяю наличие препятствий слева и справа и соответственно меняю последовательности:

        if (is_movement_finished()) {
            if (!lobst && !robst) {
                sequence = advance_sequence; // no obstacles => go forward
            } else if (lobst && robst) {
                sequence = retreat_sequence; // obstacles left and right => go backwards
            } else if (lobst && !robst) {
                sequence = turn_right_sequence; // obstacle on the left => turn right
            } else if (!lobst && robst) {
                sequence = turn_left_sequence; // obstacle on the right => turn left
            }
            step = (step + 1) % steps_per_sequence; // if previous movement is complete, then perform the next step
            plan_next_movement(step, sequence); // execute next movement
        }

Это просто, но работает!

Письмо Деду Морозу

Любой вклад приветствуется! Присылайте ваши идеи, а пока я приведу спиок вещей, которые мне хотелось бы увидеть улучшенными:

Софт:

• Предложите мне элегантный способ получить более "живые", гладкие движения. На данный момент Петя передвигается при помощи линейной интерполяции между ключевыми позами, и было бы хорошо сделать так, чтобы походка стала менее дёрганной. Возможно, нелинейная интерполяция с заранее записанными ускорениями поможет?
• Предложите новые стратегии обхода препятствий (и их реализации!) Текущаяя реализация крайне проста, в ней читаемость кода имеет высший приоритет, а ВАУ эффект от поведения робота уже идёт на втором плане.
• Я думаю, что может быть недурно портировать код под среду ардуино для тех людей, кто не может или не хочет вызывать avr-gcc напрямую или ковыряться в регистрах конкретного камня. Если вы можете это сделать, присылайте пулл-реквест или просто форкните проект.

Железо:

Если вы добрая душа, желающая помочь с созданием версии V2 материнки, то не стесняйтесь это сделать! Вот список вещей, которые я хотел бы добавить/изменить/поправить в текущей материнке:

• Главная вещь — это добавить рубильник, чтобы отключать питание сервприводов в момент прошивки процессора;
• Убрать кварц, внутренней RC-цепочки вполне должно хватить;
• Заменить все транзисторы на версии с поверхностным монтажом;
• Заменить резистор R6 на подстроечник для более лёгкой настройки датчика препятствий;
• Предложите хорошие (маленькие и с защитой от дураков) коннекторы вместо гребёнки и предложите лучшее их месторасположение;
• Подключение ИК светодиодов под центральным сервоприводом — плохая идея. На данный момент единственно хороший вариант — это их намертво припаять, что не очень удобно;
• Чуть-чуть подвинуть электролит. Мне пришлось его наклонить, т.к. иначе центральная левая нога иначе за него задевает;
• Добавить тестовых площадок с лёгким к ним доступом осциллографом;
• Добавить пару отладочных светодиодов для отладки без осциллографа;
• Добавить площадок для пайки для всех неиспользуемых ног микропроцессора для отладки и дальнейшего расширения робота.

Заключение

Петя — это страшное веселье!