Теперь клешня и по 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;
}
Тут я кое-что недоглядел: управление реализовано так, что для калибровки приходится менять принимающую программу, в которой заложены допустимые пределы углов поворота сервомашинок, и прошивать контроллер, к которому подключена рука. Для удобства тестирования следовало бы прописать все настройки в программе джойстика, а манипулятор принимал бы только непосредственно углы серв.

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

  • +4
  • 5 декабря 2011, 21:58
  • burjui

Комментарии (19)

RSS свернуть / развернуть
+
0
здорово но дороговато…
avatar

Luan

  • 5 декабря 2011, 22:45
+
0
Ха дорговато, иди найди дешевле!
HC-05 на сегодня одни из самы дешовых БТ модулей.
avatar

skystorm

  • 6 декабря 2011, 06:59
+
0
Что то видео не показывает :(
avatar

skystorm

  • 6 декабря 2011, 07:00
+
0
Проверил… показывает.
avatar

abbivan

  • 7 декабря 2011, 10:25
+
0
Круть! Только вы уже недели как две переехали, а на видео обстановочка старая)))
avatar

Pe40rA

  • 8 декабря 2011, 15:26
+
0
Ну дык, снято было давно, а выложено только сейчас (:
avatar

burjui

  • 8 декабря 2011, 16:00
+
0
Ужос)) Тема прикольная, жаль что китайский модуль такой геморойный и непредсказуемый во всех отношениях))) НО мне кажется с CRC ты явно перемудрил)) влолне можно использовать более примитивный алгоритм, например так:
КонтрольныйБайт = (байт1+байт2+байт3..байтN)% 0xFF

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

execom

  • 8 декабря 2011, 19:26
+
+2
Одна из целей статьи — рассказать о 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 мкс времени? (:
avatar

burjui

  • 8 декабря 2011, 20:14
+
0
Ой, в последней строке листнига коммент должен быть таким:
eor r20, r19  ;  crc ^ r19 
avatar

burjui

  • 9 декабря 2011, 14:11
комментарий был удален

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

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

execom

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

burjui

  • 10 декабря 2011, 03:11
+
0
Доброго времени суток всем! у меня такой вопрос, насколько он тяжелый? а то для некоторых моделей это может быть очень принципиально. ответтье кто знает, кто использовал этот материал. и что лучше, это или просто сделать каркас из перфариованной ленты?
avatar

Marat

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

burjui

  • 17 декабря 2011, 17:29
+
0
Я вот не понимаю для чего нужно использовать функцию memmove()? В чем выигрыш, что есть не использовать данную функция?
avatar

easytech

  • 23 февраля 2013, 21:16
+
0
Ну, если комментария в коде не достаточно
avatar

burjui

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

easytech

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

easytech

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

Melkiy

  • 17 февраля 2016, 10:05

Только зарегистрированные и авторизованные пользователи могут оставлять комментарии.