Skip to content

Учебно-лабораторный комплект «Конвейерная линия»: работа с модулем ​

Введение ​

Учебно-лабораторный комплект «Конвейерная линия» — это учебная платформа для знакомства с промышленной автоматикой в миниатюре: лента приводится в движение сервоприводом Dynamixel MX-64, а управлять этим приводом можно как напрямую с микроконтроллера, так и удалённо — по сети, командами с компьютера или другого устройства. На примере этого модуля разбираются сразу три темы, важные для прикладной робототехники: работа с шиной последовательных сервоприводов (DXL), программирование на Arduino-совместимом контроллере и передача данных по протоколу UDP.

В карточке комплекта также показан вариант учебной ячейки, в котором конвейер работает вместе с манипулятором с угловой кинематикой, ресурсным набором управления РТК и модулем технического зрения. Здесь рассматривается работа с конвейером как отдельным модулем.

В статье последовательно рассмотрены три примера. В первом лента движется вперёд и назад под управлением DXL-IOT. Во втором контроллер принимает UDP-команды из локальной сети и задаёт положение привода. В третьем показано базовое управление лентой на STEM Board с библиотекой STEMLab_SDK.

Что потребуется ​

  • модуль «Конвейерная линия» с сервоприводом Dynamixel;

  • блок питания — 12 В;

  • плата DXL-PWR;

  • контроллер DXL-IOT с платой расширения Ethernet-shield;

  • два кабеля DXL;

  • кабель mini-USB;

  • компьютер с установленной Arduino IDE.

Подключение оборудования ​

  1. Соединим сервопривод конвейера с блоком DXL-PWR кабелем DXL.

  2. Вторым кабелем DXL соединим DXL-PWR с контроллером DXL-IOT (в комплекте с платой расширения Ethernet-shield).

  3. Подключим к DXL-PWR блок питания 12 В — без него сервопривод не получит силового питания и не будет вращаться, даже если контроллер работает исправно.

  4. Соединим DXL-IOT с компьютером кабелем mini-USB — через него будет загружаться программа и, при необходимости, идти отладочный обмен данными.

Если ID и другие параметры сервопривода неизвестны, их можно определить с помощью приложения AR Wizard.

Начало работы ​

Сервоприводы Dynamixel и шина DXL ​

Dynamixel — это семейство «умных» сервоприводов: в отличие от обычного RC-сервопривода, каждый из них представляет собой отдельное цифровое устройство со своим номером (ID) на общей шине. Несколько приводов Dynamixel можно объединить последовательно всего двумя проводами — линия обмена данными здесь одна на всех, а обращение идёт по адресу конкретного привода. Именно поэтому в коде мы всегда указываем ID мотора и версию протокола обмена данными: контроллер должен точно знать, к какому устройству и по каким правилам он обращается.

Работа с приводами в проекте построена на библиотеке DxlMaster2. Она скрывает низкоуровневый протокол обмена и предоставляет удобные методы вроде speed() или goalPosition().

Режимы работы: «колесо» - wheel mode и «сочленение» - joint mode ​

У сервоприводов Dynamixel есть два основных режима работы.

  • режим колеса (wheel mode) — привод вращается непрерывно с заданной скоростью, как обычный мотор; понятие «угол поворота» здесь не используется, мы задаём только скорость и направление;

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

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

Отдельно отметим метод enableTorque() — он подаёт на привод момент силы (torque), без которого мотор остаётся «расслабленным» и не реагирует на команды скорости или позиции.

Пример 1. Базовое движение ленты ​

Разберём программу, которая просто перемещает ленту вперёд-назад — на её примере удобно понять минимальный набор команд для управления Dynamixel-приводом.

Подключаем библиотеку для работы с Dynamixel-устройствами:

cpp
#include "DxlMaster2.h"

Создаём объект motor класса DynamixelMotor и указываем ID сервопривода — в данном случае 1:

cpp
DynamixelMotor motor(1);

В функции setup() запускаем шину DXL на скорости 57600 бод, указываем версию протокола (1), переводим привод в режим колеса и подаём момент:

cpp
void setup() {
  DxlMaster.begin(57600);
  motor.protocolVersion(1);
  motor.wheelMode();
  motor.enableTorque();
}

В функции loop() каждые две секунды меняем скорость и направление вращения: сначала 500, затем −500 (отрицательное значение соответствует вращению в обратную сторону):

cpp
void loop() {
  motor.speed(500);
  delay(2000);
  motor.speed(-500);
  delay(2000);
}

Полный код программы:

cpp
#include "DxlMaster2.h"

DynamixelMotor motor(1);

void setup() {
  DxlMaster.begin(57600);
  motor.protocolVersion(1);
  motor.wheelMode();
  motor.enableTorque();
}

void loop() {
  motor.speed(500);
  delay(2000);
  motor.speed(-500);
  delay(2000);
}

Пример 2. Управление лентой по UDP ​

Второй пример сложнее: лента управляется удалённо — контроллер принимает команды по локальной сети и переводит привод в заданное положение. Здесь используется режим сочленения (joint mode): привод поворачивается на угол, а не крутится вперёд-назад.

Что такое UDP ​

UDP (User Datagram Protocol) — один из базовых протоколов передачи данных в компьютерных сетях, наравне с TCP. В отличие от TCP, UDP не устанавливает постоянное соединение и не гарантирует доставку каждого пакета — зато он быстрее и проще в реализации. Для управления в реальном времени, когда важна скорость реакции, а потеря отдельной команды не критична, UDP подходит хорошо.

Подключаемые библиотеки ​

cpp
#include <Ethernet.h>
#include <EthernetUdp.h>
#include <Regexp.h>
#include "DxlMaster2.h"

Ethernet и EthernetUdp отвечают за работу с сетью и приём UDP-пакетов, Regexp — за разбор текста входящих сообщений с помощью регулярных выражений, DxlMaster2 — за управление сервоприводом.

Настройка сети ​

cpp
byte mac[] = { 0xDE, 0xAD, 0xBE, 0xEF, 0xFE, 0xED };
IPAddress ip(192, 168, 42, 101);

MAC-адрес — уникальный идентификатор сетевого устройства, IP-адрес — его адрес в локальной сети. Важно: если в вашей сети работает несколько таких устройств одновременно, у каждого должны быть свои MAC- и IP-адреса, иначе возникнет конфликт.

Также создаём объект мотора и настраиваем UDP-порт — своего рода «дверь», через которую контроллер будет принимать входящие сообщения:

cpp
DynamixelMotor motor(1);

unsigned int localPort = 8888;
EthernetUDP Udp;

Инициализация ​

В функции setup() повторяется настройка сервопривода, но на этот раз в режиме сочленения (joint mode) — приводу можно указывать конкретное положение:

cpp
DxlMaster.begin(57600);
motor.protocolVersion(1);
motor.jointMode();
motor.enableTorque();

Дальше инициализируется сетевой модуль: указывается пин, к которому подключена Ethernet-плата, адреса из предыдущего шага и порт для приёма UDP-сообщений:

cpp
Ethernet.init(10);
Ethernet.begin(mac, ip);
Udp.begin(localPort);

Формат управляющих сообщений и их разбор ​

Все команды на движение вынесены в отдельную функцию recieveUDP(), которая вызывается в каждом цикле loop(). Она проверяет, не пришёл ли новый пакет, и если да — считывает его в буфер:

cpp
void recieveUDP(){
  int packetSize = Udp.parsePacket();
  if(packetSize > 0){
    char packetBuffer[UDP_TX_PACKET_MAX_SIZE];
    Udp.read(packetBuffer, UDP_TX_PACKET_MAX_SIZE);

Дальше текст сообщения сверяется с шаблоном регулярного выражения "s:(%d+)#". Регулярное выражение — это способ описать «форму» строки, которую мы ищем: буква s, за ней двоеточие, затем одна или несколько цифр (это и есть нужное нам число — новое положение привода) и завершающая решётка. Например, сообщению «s:512#» такой шаблон соответствует, а число 512 как раз попадёт на сервопривод:

cpp
    MatchState ms;
    ms.Target(packetBuffer);
    char result = ms.Match("s:(%d+)#", 0);
    char m[20];
    if(result == REGEXP_MATCHED){
      ms.GetMatch(m);
      String val = ms.GetCapture(m, 0);
      motor.goalPosition((uint16_t)val.toInt());
    }
    packetBuffer[UDP_TX_PACKET_MAX_SIZE] = 0;
  }

  delay(35);
}

Если совпадение найдено, извлечённое число преобразуется в целое (toInt()) и передаётся в motor.goalPosition() — тем самым привод получает новую целевую позицию и начинает поворот к ней.

Полный код программы**:**

cpp
#include <Ethernet.h>
#include <EthernetUdp.h>
#include <Regexp.h>
#include "DxlMaster2.h"

byte mac[] = {
  0xDE, 0xAD, 0xBE, 0xEF, 0xFE, 0xED
};
IPAddress ip(192, 168, 42, 101);

DynamixelMotor motor(1);

unsigned int localPort = 8888;
EthernetUDP Udp;

void setup() {
  DxlMaster.begin(57600);
  motor.protocolVersion(1);
  motor.jointMode();
  motor.enableTorque();

  Ethernet.init(10);
  Ethernet.begin(mac, ip);
  Udp.begin(localPort);
}

void loop() {
  recieveUDP();
}

void recieveUDP(){
  int packetSize = Udp.parsePacket();
  if(packetSize > 0){
    char packetBuffer[UDP_TX_PACKET_MAX_SIZE];
    Udp.read(packetBuffer, UDP_TX_PACKET_MAX_SIZE);
    MatchState ms;
    ms.Target(packetBuffer);
    char result = ms.Match("s:(%d+)#", 0);
    char m[20];
    if(result == REGEXP_MATCHED){
      ms.GetMatch(m);
      String val = ms.GetCapture(m, 0);
      motor.goalPosition((uint16_t)val.toInt());
    }
    packetBuffer[UDP_TX_PACKET_MAX_SIZE] = 0;
  }

  delay(35);
}

Пример 3. Базовое движение ленты на STEM Board ​

Разберём программу, которая перемещает ленту вперёд и назад. На её примере удобно изучить минимальный набор команд для управления Dynamixel-приводом с помощью STEM Board и библиотеки STEMLab_SDK.

Подключение библиотеки и настройка

Подключим библиотеку для работы с Dynamixel-устройствами:

cpp
#include "STEMLab_SDK.h"

Создадим объект sdk класса STEMLab_SDK, задав скорость обмена 57600 бод и протокол версии 1.0:

cpp
#define BAUDRATE 57600
#define PROTOCOL 1.0
STEMLab_SDK sdk(BAUDRATE, PROTOCOL);

Настройка режима колеса

Функция setMode() записывает нулевые значения в регистры CW и CCW angle limit (адреса 6 и 8). Это включает режим непрерывного вращения:

cpp
void setMode() {
  sdk.write2Byte(SERVO_ID, 6, 0);
  sdk.write2Byte(SERVO_ID, 8, 0);
}

Инициализация сервопривода

В setup() запускается последовательный порт, инициализируется SDK, устанавливается режим колеса и включается момент сервопривода:

cpp
void setup() {
  Serial.begin(57600);
  sdk.init();
  setMode();
  sdk.write1Byte(SERVO_ID, 24, 1);
}

Изменение скорости и направления

В loop() каждые две секунды передаются значения скорости 1300 и 300. В режиме колеса эти значения задают вращение в противоположных направлениях:

cpp
void loop() {
  sdk.write2Byte(SERVO_ID, 32, vel1);
  delay(2000);
  sdk.write2Byte(SERVO_ID, 32, vel2);
  delay(2000);
}

Полный код программы

cpp
#include "STEMLab_SDK.h"

#define SERVO_ID 1
#define BAUDRATE 57600  // скорость порта DXL
#define PROTOCOL 1.0

STEMLab_SDK sdk(BAUDRATE, PROTOCOL);

uint16_t vel1 = 1300;
uint16_t vel2 = 300;

void setMode() {
  sdk.write2Byte(SERVO_ID, 6, 0);
  sdk.write2Byte(SERVO_ID, 8, 0);
}

void setup() {
  Serial.begin(57600);
  sdk.init();
  setMode();
  sdk.write1Byte(SERVO_ID, 24, 1);
}

void loop() {
  sdk.write2Byte(SERVO_ID, 32, vel1);
  delay(2000);
  sdk.write2Byte(SERVO_ID, 32, vel2);
  delay(2000);
}

Перед запуском проверьте, что ID сервопривода равен 1, а скорость и версия протокола совпадают с его настройками. Освободите ленту от предметов и не касайтесь движущихся частей.

Возможные проблемы и их решение ​

  • Лента не двигается — проверим, подключён ли блок питания 12 В к DXL-PWR: без силового питания привод не получает момент, даже если код загружен и работает исправно.

  • Контроллер не виден в сети или не приходят UDP-пакеты — убедимся, что IP-адрес контроллера не совпадает с адресом другого устройства в сети и что компьютер, с которого отправляются команды, находится в той же локальной сети.

  • Не удаётся определить ID сервопривода — используем приложение AR Wizard: оно показывает текущие параметры подключённых приводов Dynamixel.