Всем хабраконструкторам, привет!

Пришла мне как-то в голову дурацкая мысль: собрать девайс, который бы молотком забивал гвозди. Просто ради демонстрации работы сервопривода. Алгоритм простой: даём команду на поднятие молотка, ждём пока он поднимется, отпускаем молоток; и так пока гвоздь не будет забит. Но как узнать, что молоток поднялся и что гвоздь забит, не пользуясь дополнительными датчиками? Спросить у «глупого» сервопривода! Как именно это сделать — об этом и пойдёт речь в статье.


Что такое сервопривод? Наверное, все знают, но на всякий случай: это привод, который в отличие от мотора постоянного тока не просто крутится пока подаётся напряжение, а стремится повернуться к заданному углу и удержаться в этом положении. Угол устанавливается с помощью ШИМ (PWM)-сигнала. Сервопривод стремится к определённому положению, а следовательно должен знать своё собственное. Перед началом сборки я был уверен, что запросить текущий угол будет проще простого и это возможно «из коробки». Не тут то было. Но обо всём по порядку.

Итак, предполагаемый девайс: сервопривод с прикреплённым к нему молотком на небольшом постаменте для равновесия. Сервопривод подключается к Arduino через IO Shield, а микроконтроллер исполняет алгоритм:
  • Установить сервоприводу определённый угол для поднятия молотка
  • Бездействовать пока сервопривод не сообщит, что угол достигнут
  • Отключить питание сервопривода, чтобы молоток упал на гвоздь
  • Прочитать угол в упавшем положении
  • Если угол после падения несколько раз подряд не изменился — значит гвоздь перестал вколачиваться. Предположительно он забит — прекращаем исполнение
  • Если угол изменился, начинаем сначала

Берём исходные части:



Пилим и скручиваем:



Приступаем к написанию прошивки для Arduino… Довольно быстро становится понятно, что установить определённый угол для сервы — не проблема. В частности, это позволяет сделать стандартная библиотека Servo, которая из заданного в градусах угла формирует соответствующий PWM-сигнал. А вот с чтением — проблема: функции для этого нет.

Быстро погуглив проблему, нашёл кучу сообщений на форумах, где на этот вопрос авторитетно отвечали: «Это не возможно! Сервоприводы — это write-only устройства». Меня это привело в замешательство, я интуитивно чувствовал, что достать эти данные как-то просто можно.

Матчасть

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

То есть, у нас есть потенциометр, по сигналу с которого можно определить текущий угол. Осталось только разобрать сервопривод и подключиться в нужном месте. Разбираем:



Сразу скажу, что сервопривод с фотографии я безвозвратно сломал в процессе разборки. Не нужно было вообще выламывать плату с электроникой, достаточно просто снять заднюю крышку, которая держится на 4-х винтах. Но сразу это было не очевидно, и чтобы понять куда на плате припаян потенциометр, пришлось пожертвовать одним приводом.

Вот как припаян потенциометр на сервоприводах от DFRobot:



Нам нужен сигнал с бегунка, который меняется в зависимости от угла поворота от минимального до максимального напряжения. Берём мультиметр, вращаем шпиндель и смотрим: каким углам какой сигнал соответствует. Для моей сервы углу в 0° соответствует напряжение 0.43 В, а максимальному углу поворота в 180° соответствует напряжение 2.56 В.

Аккуратно припаиваем новый сигнальный провод.



Подключаем его к аналоговому входу A5 на Arduino. Закрываем крышку. Пишем программу:

#include <Servo.h>

// разрешене аналогого порта
#define A_MAX 1024

// опорное напряжение на котором работает серва
#define A_VREF 5

// предельные уровни сигнала с сервы
#define A_VMIN 0.43
#define A_VMAX 2.56

Servo servo;

int lastHitAngle = 0;
int hitAngleMatches = 0;
bool jobDone = false;

/*
 * Возвращает текущий угол поворота сервы исходя
 * из сигнала с его потенциометра
 */
int realAngle()
{
    return map(
            analogRead(A5), 
            A_MAX * A_VMIN / A_VREF, 
            A_MAX * A_VMAX / A_VREF, 
            0, 180);
}

void setup()
{
}

void loop()
{
    if (jobDone)
        return;

    // включаем серву и просим повернуться до положения 70°
    servo.attach(6);
    servo.write(70);

    // ждём поворота. 5° запаса на всякие погрешности
    while (realAngle() < 65)
        ;

    // бросаем молоток и ждём немного пока он успокоится
    servo.detach();
    delay(1500);

    // запоминаем угол после падения и сопоставляем его с
    // предыдущим
    int hitAngle = realAngle();
    if (hitAngle == lastHitAngle)
        ++hitAngleMatches;
    else {
        lastHitAngle = hitAngle;
        hitAngleMatches = 0;
    }

    // если угол не менялся 5 раз — мы закончили
    if (hitAngleMatches >= 5)
        jobDone = true;
}


Включаем, пробуем, работает!



Что делать с полученным опытом — вариантов много: можно сделать контроллер вроде того, что используется на кораблях для установки тяги (полный вперёд / полный назад); можно использовать серву с обратной связью как элемент автономного рулевого управления какой-нибудь машины; можно много всего. Да прибудет со всеми нами фантазия!