Думаю, многие помнят одно из недавних наших творений — клешню из ПКЛ (поликапролактона), и кто-то, возможно, даже читал о Bluetooth-модулях HC-05. Так вот, мы решили сделать клешню управляемой по Bluetooth с Arduino.

Я использовал предельно простой метод — использовать HC-05 как радиоудлинитель UART, и гонять по нему данные. Для начала, нужно было определиться с тем, какие данные нужно передавать с… эээ… «джойстика»? Короче, вот этой штуки:

Решил переложить обработку данных на саму клешню, а с «джойстика» клешне слать положения потенциометров. Для начала нужно было наладить связь. Можно было пойти сложным путём — при старте Arduino переводить HC-05 в режим AT-команд и запускать подключение, но есть способ проще: использовать функцию автоподключения. В даташите написано, что нужно сначала сделать AT+CMODE=1 (подключаться к любому устройству), потом подключиться к нужному устройству и сделать AT+CMODE=0 — и тогда модуль будет подключаться к нужному устройству автоматически. Брехня чистой воды! В очередной раз убедился, что документацию писали несколько разных китайцев, необязательно даже разработчиков модуля. На деле, как я и думал, нужно выполнить AT+BIND=<адрес устройства> — привязать модуль к устройству, а потом сделать AT+CMODE=0, чтобы модуль автоматически подключался к нему. Как говорится, доверяй, но проверяй (:

После чтения китайшита я подключил модули к ардуинам у «джойстика» (Модуль A) и клешни (Модуль B) и проделал с модулями следующие манипуляции:

• Подключил к обоим модулям питание и перевёл их в AT-режим.
• Подключился к модулям через терминалку.
• Сбросил у обоих настройки на дефолтные командой AT+ORGL.
• Перевёл модуль A в режим master (инициирует подключение) и дал ему имя:

  AT+ROLE=1 ; режим master
  AT+NAME=joystick

• То же проделал с модулем B, только с режимом slave (ждёт подключения):

  AT+ROLE=0 ; режим slave
  AT+NAME=manipulator

• Узнал адреса модулей:
  A:

  AT+ADDR?
  +ADDR:11:4:290255
  OK

  B:

  AT+ADDR?
  +ADDR:11:4:291093
  OK

• Наказал модулю B не принимать незваных гостей:

  AT+RMAAD  ; очистить список авторизованных устройств
  AT+BIND=11,4,290255  ; привязать модуль B к модулю A
  AT+CMODE=0  ; в режиме slave принимать подключение только от модуля A

• Модулю A наказал подключаться только к модулю B:

  AT+RMAAD
  AT+BIND=11,4,291093  ; в режиме master подключаться только к модулю B
  AT+CMODE=0

Всё, теперь модуль A при включении будет искать модуль B и автоматически подключаться к нему (и переподключаться при обрыве связи). Теперь нужно написать программы для отсылки показаний потенциометров клешне и для приёма и обработки этих показаний. Начнём с отправки.

Быстродействие сервоприводов ограничено, так что вполне достаточно будет передавать данные о положении потенциометров 100 раз в секунду. Только нужно определиться, как их передавать. Дело в том, что если просто слать номер потенциометра и считанное значение, приёмник не сможет узнать, где в потоке принятых байт начинается номер, а где — значение.

Вот тут нам пригодится такая замечательная вещь, как CRC. Если коротко, то это контрольная сумма данных, имеющая фиксированную длину. Если сравнить контрольные суммы двух наборов данных, то можно с большой вероятностью говорить об идентичности этих наборов данных. В данном случае я выбрал CRC-8, 8-битный алгоритм CRC, который для данных любой длины генерирует контрольную сумму длиной 8 бит, или 1 байт. Вот её-то мы и будем посылать после наших данных — тогда принимающая сторона сможет вычислять текущее значение CRC для принятых данных и сравнивать с последним принятым байтом: если значения равны, значит принятые ранее байты содержат правильные данные. Сам алгоритм прост, как устройство пуговицы — он побитово сканирует входные данные и применяет к их старшему биту операцию «исключающее ИЛИ» с участием специального двоичного многочлена (CRC polynomial), в котором вся соль алгоритма, и после каждой такой операции сдвигает результат влево. Звучит страшно? Двоичный многочлен — это всего-то 8-битное число, 1 байт. Да и нам его придумывать не нужно, за нас уже всё придумали программисты-математики. Вот функция для вычисления CRC-8 для одного байта данных:

uint8_t crc8(uint8_t crc, uint8_t data, uint8_t polynomial)
{
  crc ^= data;

  for (int i = 0; i < 8; ++i)
    crc = (crc << 1) ^ ((crc & 0x80) ? polynomial : 0);

  return crc;
}

Если обрабатываем первый байт данных, то передаём этой функции нулевой crc, иначе - результат применения функции к предыдущему байту данных. Полином я выбрал рекомендованный институтом CCITT - 0x07, после чего написал вспомогательную функцию:

uint8_t crc8_ccitt(uint8_t crc, uint8_t data)
{
  return crc8(crc, data, 0x07);
}

Теперь вычисление CRC-8 для блока данных выглядит так:

uint8_t crc = crc8_ccitt(0, byte1);
crc = crc8_ccitt(crc, byte2);
crc = crc8_ccitt(crc, byte3);
...
crc = crc8_ccitt(crc, byteN);

Отсылка данных потенциометра происходит в следующем порядке побайтно:

1. Номер потенциометра (их по-любому меньше 256)
2. Младший байт значения, считанного с потенциометра: analogRead() возвращает 10-битное значение (0..1023), которое приходится разбивать на два куска.
3. Старший байт значения.
4. CRC-8 от предыдущих трёх байтов.

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

Вот так и происходит приём данных: сдвигаем принятые байты к началу буфера, записываем в конец буфера считанный байт и проверяем, является ли он результатом вычисления CRC-8 от предыдущих трёх байт. Если да, то забираем из буфера данные и обрабатываем. Конечно, мой метод отличается от классических, когда CRC служит только для проверки целостности данных, а разбиение на пакеты происходит с использованием комбинации символов, которая гарантированно не может встретиться в правильных данных, или предварительной отправки длины пакета. Но в данном случае проще было сделать так.

После всех этих размышлений родился следующий код для "джойстика":

/* Для удобной работы с потенциометром нарисовал такую структурку */
struct PotData
{
  uint8_t pin; // номер аналогового пина
  int value; // последнее считанное значение
  bool changed; // признак того, что считанное значение изменилось

  void read()
  {
    int old_value = value;
    value = analogRead(pin);
    /* Попытка отбросить часть случайных флуктуаций на аналоговом входе */
    changed = abs(value - old_value) > 2;
  }

  void sendOverSerial()
  {
    /* Считаем CRC для отправляемых данных */
    uint8_t crc = crc8_ccitt(0, pin);
    uint8_t value_low = lowByte((uint16_t)value),
            value_high = highByte((uint16_t)value);

    crc = crc8_ccitt(crc, value_low);
    crc = crc8_ccitt(crc, value_high);

    /* Шлём данные и CRC */
    Serial.write(pin);
    Serial.write(value_low);
    Serial.write(value_high);
    Serial.write(crc);
  }
};

enum { POTS_AMOUNT = 6 };

PotData pot_data[POTS_AMOUNT] =
{
  { 0, 0, false },
  { 1, 0, false },
  { 2, 0, false },
  { 3, 0, false },
  { 4, 0, false },
  { 5, 0, false },
};


void setup()
{
  Serial.begin(38400);
}


void loop()
{
  /* 100 раз в секунду читаем и шлём значения с аналоговых входов */
  for (int i = 0; i < POTS_AMOUNT; ++i)
  {
    pot_data[i].read();
    pot_data[i].sendOverSerial();
  }

  delay(10);
}


uint8_t crc8_ccitt(uint8_t crc, uint8_t data)
{
  return crc8(crc, data, 0x07);
}


uint8_t crc8(uint8_t crc, uint8_t data, uint8_t polynomial)
{
  crc ^= data;

  for (int i = 0; i < 8; ++i)
    crc = (crc << 1) ^ ((crc & 0x80) ? polynomial : 0);

  return crc;
}

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

#include <Servo.h>

struct Joint
{
  uint8_t servo_pin;
  int pot_min, pot_max;
  int servo_min, servo_max;
  Servo servo;
};

Joint joints[] =
{
  /* захват */
  { 2,
    10, 600,
    0, 180,
    Servo()
  },

  /* поворот кисти */
  { 4,
    0, 1024,
    0, 180,
    Servo()
  },

  /* наклон кисти */
  { 3,
    0, 550,
    180, 0,
    Servo()
  },

  /* локоть */
  { 5,
    350, 950,
    180, 0,
    Servo()
  },

  /* поворот */
  { 7,
    0, 1024,
    180, 0,
    Servo()
  },

  /* плечо */
  { 6,
    0, 1024,
    180, 0,
    Servo()
  },
};

enum { JOINTS_AMOUNT = sizeof(joints) / sizeof(joints[0]) };

void setup()
{
  Serial.begin(38400);

  for (int i = 0; i < JOINTS_AMOUNT; ++i)
  {
    Joint &joint = joints[i];
    joint.servo.attach(joint.servo_pin);
  }

  pinMode(13, OUTPUT); // будем мигать светодиодом L при приёме данных
}

void loop()
{
  /* Вот и наш буфер для приёма данных размером в 4 байта */
  static uint8_t packet[4];

  while (Serial.available() > 0)
  {
    /* Функция memmove() копирует содержимое одного блока памяти
     * в другой, при этом позволяя указывать перекрывающиеся блоки,
     * а нам именно это и нужно. Кстати, функция memcpy() работает
     * аналогично, но для перекрывающихся блоков работает неправильно,
     * и это не баг, а фича для ускорения работы.
     */
    memmove(packet, packet + 1, sizeof(packet) - 1);
    /* Данные сдвинули - принимаем свежие, в конец буфера */
    packet[3] = Serial.read();
    /* Вычисляем CRC для первых трёх байт буфера */
    uint8_t crc = crc8_ccitt_block(packet, sizeof(packet) - 1);

    /* Если последний принятый байт - CRC от предыдущих трёх,
     * значит, мы приняли корректные данные потенциометра.
    */
    if (crc == packet[3])
    {
      /* Мигнём L-кой, сигнализируя успешный приём данных */
      digitalWrite(13, !digitalRead(13));
      /* Выдёргиваем из буфера номер потенциометра
       * и считанное с него значение.
       */
      uint8_t pot_index = packet[0];
      int pot_angle = packet[1] | packet[2] << 8;

      /* В обработке всё то же самое, ничего нового */
      if (0 <= pot_index && pot_index < JOINTS_AMOUNT)
      {
        Joint &joint = joints[pot_index];

        int servo_angle = map(pot_angle, joint.pot_min, joint.pot_max,
        joint.servo_min, joint.servo_max);

        bool inverse = joint.servo_min > joint.servo_max;

        servo_angle = constrain(servo_angle,
          (inverse ? joint.servo_max : joint.servo_min),
          (inverse ? joint.servo_min : joint.servo_max));

        joint.servo.write(servo_angle);
      }
    }
  }
}

/* Вспомогательная функция вычисления CRC для массива байтов */
uint8_t crc8_ccitt_block(const uint8_t *data, size_t length)
{
 uint8_t crc = 0;

 for (size_t i = 0; i < length; ++i)
    crc = crc8_ccitt(crc, data[i]);

 return crc;
}

uint8_t crc8_ccitt(uint8_t crc, uint8_t data)
{
 return crc8(crc, data, 0x07);
}

uint8_t crc8(uint8_t crc, uint8_t data, uint8_t polynomial)
{
 crc ^= data;

 for (int i = 0; i < 8; ++i)
    crc = (crc << 1) ^ ((crc & 0x80) ? polynomial : 0);

 return crc;
}

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

Ну, а теперь демонстрация прототипа дистанционной руки-убийцы: