Учебно-лабораторный комплект «Конвейерная линия»: работа с модулем
Введение
Учебно-лабораторный комплект «Конвейерная линия» — это учебная платформа для знакомства с промышленной автоматикой в миниатюре: лента приводится в движение сервоприводом 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.
Подключение оборудования
Соединим сервопривод конвейера с блоком DXL-PWR кабелем DXL.
Вторым кабелем DXL соединим DXL-PWR с контроллером DXL-IOT (в комплекте с платой расширения Ethernet-shield).
Подключим к DXL-PWR блок питания 12 В — без него сервопривод не получит силового питания и не будет вращаться, даже если контроллер работает исправно.
Соединим 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.