Теперь клешня и по Bluetooth


Думаю, многие помнят одно из недавних наших творений — клешню из ПКЛ (поликапролактона), и кто-то, возможно, даже читал о 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;
}

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

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


19 комментариев на «“Теперь клешня и по Bluetooth”»

    • Ха дорговато, иди найди дешевле!
      HC-05 на сегодня одни из самы дешовых БТ модулей.

    • Ну дык, снято было давно, а выложено только сейчас (:

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

    Может я что-то не понимаю, но период следования фазозадающих импульсов у сервоприводов — 20мс (50Гц) зачем слать 100 раз в секунду??

    Кстати для тех у кого с китайским проблема можно посмотреть английский вариант даташита на HC-05)))

    • Эээ, так даташит, указанный в статье — на английском. А «китайшит» — это такой тонкий намёк на его происхождение и качество.
      Что касательно 100 раз в секунду — так это я с потолка цифру взял, можно было бы и 50 раз в секунду, просто я высокий, и до потолка мне дотянуться не трудно. Всё вы правильно понимаете (:

  2. Доброго времени суток всем! у меня такой вопрос, насколько он тяжелый? а то для некоторых моделей это может быть очень принципиально. ответтье кто знает, кто использовал этот материал. и что лучше, это или просто сделать каркас из перфариованной ленты?

    • Вес ПКЛ в нашей клешне — около 0.6 кг, можете оценить его плотность. Для моделей с особыми требованиями по весу его лучше не применять, т.к. он гибкий, когда тонкий, но тяжёлый, когда толстый.

    • Я знаю что эта за функция я просто не понял для чего она. То что она копирует один блок в другой понятно, но что это дает?
      Ладно, не в этом дело. Я не работаю с Ардуино, я не знаю ее библиотеку serialport, но когда я использую переменную типа uint8_t для буфера принятых значении то они потом не распознаются в программе. Только когда я пишу char все работает. Странно как твой код работает с переменными uint8_t просто интересно)) я знаю что это тоже самое что и char только через typedef переделана)

    • «Я знаю что эта за функция я просто не понял для чего она. То что она копирует один блок в другой понятно, но что это дает?»
      Можешь не отвечать, извини. В данном случае понял)

  3. Привет. Занялся интересным проектом по замене старой аналоговой приборной панели в машине на новую цифровую работающую по кан шине. Двигатель управляется эбу vems(vems.hu), передает и принимает данные по rs232, есть описание протокола вывода основных данных для подключения всяких доп показометров. Пакет данных имеет длинну 5 байт, Первый байт номер канала данных, второй всегда a3(hex), третий и четвертый вывод данных, пятый контольная сумма. Пытался использовать код от манипулятора по блютуз, но что то не получается из пакета выдергивать определенный байт, если вписать в Serial.print(packet[5]); то выводятся байты по очереди, если вписать от[1-4] то в мониторе ардуино принимает 0, а в другом терминале принимает значение (30). Буфер до пяти байт расширил, подсчет crc до четырех байт расширил, но есть подозрение что косяк в том, что эбу шлет данные в hex и uint8_t для этих целей не совсем подходит, а перевести на uint16 с работающим подсчетом crc у меня не получается. Вот инструкция к протоколу: http://www.vems.hu/files/MembersPage/NanassyPeter/AIM_support/AIM-ECU%20protocol.pdf
    Если есть возможность, то помогите пожалуйста 🙂

  4. Ужос)) Тема прикольная, жаль что китайский модуль такой геморойный и непредсказуемый во всех отношениях))) НО мне кажется с CRC ты явно перемудрил)) влолне можно использовать более примитивный алгоритм, например так:

    КонтрольныйБайт = (байт1+байт2+байт3..байтN)% 0xFF

    это не намного хуже чем CRC8 зато на сколько проще, а оптимизация для программ для ардуино, особенно в больших проектах весьма актуальна)

    • Одна из целей статьи — рассказать о CRC (:
      Штука очень полезная, но почти не известная начинающим. Что до оптимизации — ATmega168 не имеет аппаратного деления, и если делить не на степени двойки (этот случай компилятор может оптимизировать в сдвиг вправо на эту степень), то компилятор применит программное деление, которое жутко медленное, к тому же делить придётся 16-разрядное число на 8-разрядное. Так что вряд ли тут будет выигрыш, т.к. выражение

      crc = (crc << 1) ^ ((crc & 0x80) ? polynomial : 0);

      компилятор оптимизирует до примерно такого кода:

      ;;; Положим, в r20 - текущее значение crc
      ;; Пропуск следующей инструкции, если бит не установлен - до 3 тактов
      sbrc r20, 7   ; (crc & 0x80) ? 
      ;; Загрузка константы - 1 такт
      ldi r19, 0x07 ; polynomial
      ;; Загрузка константы - 1 такт
      ldi r19, 0    ; 0
      ;; Логический сдвиг влево на 1 бит - 1 такт
      lsl r20       ;  crc = crc << 1
      ;; Исключающее или - 1 такт
      eor r20, r19  ;  crc = crc << 1
      

      Итого имеем 7 тактов на тело цикла, которое выполняется 8 раз — имеем 56 тактов + накладные расходы на цикл около 8 * 3 = 24 такта и 2 такта на crc ^= data и return crc. Итого грубо 90 тактов, выполняемых 3 раза (для трёх байтов данных) = 270 тактов или 17 микросекунд. Размер кода функции не превышает 30 байт.

      Это очень дорого — 30 байт кода и 17 мкс времени? (:

    • Ой, в последней строке листнига коммент должен быть таким:

      eor r20, r19  ;  crc ^ r19 

Добавить комментарий

Arduino

Что такое Arduino?
Зачем мне Arduino?
Начало работы с Arduino
Для начинающих ардуинщиков
Радиодетали (точка входа для начинающих ардуинщиков)
Первые шаги с Arduino

Разделы

  1. Преимуществ нет, за исключением читабельности: тип bool обычно имеет размер 1 байт, как и uint8_t. Думаю, компилятор в обоих случаях…

  2. Добрый день! Я недавно начал изучать программирование под STM32 и ваши уроки просто бесценны! Хотел узнать зачем использовать переменную типа…

3D-печать AI Android Arduino Bluetooth CraftDuino DIY IDE iRobot Kinect LEGO OpenCV Open Source Python Raspberry Pi RoboCraft ROS swarm ИК автоматизация андроид балансировать бионика версия видео военный датчик дрон интерфейс камера кибервесна манипулятор машинное обучение наше нейронная сеть подводный пылесос работа распознавание робот робототехника светодиод сервомашинка собака управление ходить шаг за шагом шаговый двигатель шилд юмор

OpenCV
Робототехника
Будущее за бионическими роботами?
Нейронная сеть - введение