Глава 2. Обзор программной составляющей

Подготовка среды разработки

Универсальный вычислительный контроллер DXL-IoT является подобной Arduino Mega 2560 платой. Среда программирования Arduino IDE, позволяет реализовать возможность разработки программного кода, для платы DXL-IoT, применяя на практике те же подходы и библиотеки, что и для Arduino Mega 2560. Для работы с контроллером рекомендуется использовать среду разработки Arduino IDE версии не ниже 1.8, которую необходимо скачать с сайта Arduino.cc и инсталлировать в операционную систему штатными средствами. Поскольку на контроллере имеется модуль беспроводной связи Wi-Fi / Bluetooth, необходимо добавить в среду разработки поддержку данного модуля. Для этого нужно перейти в меню «Файл» - «Настройки» - «Дополнительные ссылки для Менеджера плат» и в открывшемся окне (Рис. 2.1) вставить следующую ссылку: https://dl.espressif.com/dl/package_esp32_index.json

Рис. 2.1. Добавление ссылки для менеджера плат

В обоих окнах нажать кнопку «ОК».

После указания дополнительной ссылки для менеджера плат требуется перейти в раздел «Инструменты» - «Платы» - «Менеджер плат», в строке поиска ввести esp32 и установить найденный пакет «esp32 by Espressif Systems» (Рис. 2.2).

Рис. 2.2. Установка пакета поддержки плат семейства ESP32

В результате этих действий, в меню «Инструменты» - «Плата» появится раздел «ESP32 Arduino», в котором для работы со встроенным модулем беспроводной связи Wi-Fi / Bluetooth необходимо выбрать «ESP32 Dev Module».

Отличительной особенностью контроллера DXL-IoT является поддержка интерфейса Dynamixel, для работы с которым разработаны специальные библиотеки. Библиотеки для работы с Dynamixel-протоколом можно скачать с сайта appliedrobotics.ru из раздела «Учебные материалы» – «Программное обеспечение». Существует 2 способа работы контроллера в цепи Dynamixel – совместимых устройств: в качестве

«Master-устройства» (ведущий контроллер) и «Slave-устройства» (ведомое Dynamixel-совместимое устройство).

Работа с Dynamixel – совместимыми устройствами, библиотеки DxlMaster и DxlMaster2

Dynamixel – совместимые устройства - сервоприводы и датчики, работающие на протоколе Dynamixel, подключаются к интерфейсу на основе последовательного порта UART в режиме полудуплекса. Для управления устройствами Dynamixel с универсального вычислительного контроллера DXL-IoT. Необходимо использовать библиотеку «DxlMaster» для работы по протоколу Dynamixel 1.0, а для работы по протоколу Dynamixel 2.0 – библиотеку «DxlMaster2».

Загрузить библиотеки можно с сайта ООО «Прикладная робототехника» (appliedrobotics.ru) или по ссылке: https://github.com/AppliedRobotics/. Библиотеки при скачивании поставляются в виде zip-библиотеки Arduino и устанавливаются штатными средствами Arduino IDE (Меню «Скетч» -> «Подключить библиотеку» -> «Добавить .ZIP библиотеку»).

Библиотеки “DxlMaster” и “DxlMaster2” содержат следующие примеры (меню «Файл» -> «Примеры»):

blink_led – включение и выключение светодиодов сервоприводов, работающх по протоколу Dynamixel.
change_baudrate – изменение скорости передачи данных на шине Dynamixel.
console – терминал, позволяющий из командной строки найти и настроить Dynamixel – совместимые устройства в сети.
joint_mode – управление сервоприводами по положению (режим шарнирного сочленения).
wheel_mode – управление сервоприводами по скорости (режим колеса).
wheel_mode_low_level – управление сервоприводами на низком уровне (изменением значений регистров).

Инициализация библиотеки

Во всех случаях использования библиотек “DxlMaster” и “DxlMaster2” необходимо добавить в программу и инициализировать. Для добавления библиотеки необходимо добавить следующий заголовочный файл в начале программы:

#include “DxlMaster.h” или #include “DxlMaster2.h”

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

DxlMaster.begin(int baudrate);

где baudrate – скорость обмена данными с устройствами на шине Dynamixel (см. документацию к конкретной модели сервопривода). Чаще всего используются скорости 57 600 бод и 1 000 000 бод. Скорость обмена данными с Dynamixel – совместимым устройством может быть изменена через программу ROBOTIS Dynamixel Wizard, либо используя пример change_baudrate.

Подключение произвольных устройств, класс DynamixelDevice

Обмен данными с Dynamixel – совместимыми устройствами происходит путем записи и чтения регистров в памяти такого устройства. Для этого предназначен класс DynamixelDevice библиотек “DxlMaster” и “DxlMaster2”.

Общие методы класса DxlMaster:

DxlMaster(uint8_t id) – конструктор, id – уникальный номер устройства

Dynamixel в сети (см. документацию на устройство). Может быть изменен в программе ROBOTIS Dynamixel Wizard.

DynamixelStatus init() – поиск и инициализация устройства. Если найдено устройство с адресом id и скоростью обмена, соответствующей текущим настройкам шины, возвращает 0, иначе – код ошибки.
DynamixelStatus changeId(uint8_t id) - изменение id устройства (для этого устройство все равно должно быть инициализировано с исходным id).
uint16_t model() – возвращает номер модели устройства.
uint8_t firmware() – возвращает версию прошивки устройств.
void communicationSpeed(uint32_t aSpeed) – изменение скорости обмена данными с устройством.

Обмен данными:

Все методы для обмена данными возвращают статус выполнения команды DynamixelStatus, принимающий значение «0» в случае успеха или выдающий номер стандартной ошибки шины Dynamixel, в случае сбоя обмена данными.

Методы чтения данных выполнены в виде шаблонов для чтения регистров разной длины (чаще всего в Dynamixel – совместимых устройствах применяются регистры типов uint8_t, uint16_t), например:

DynamixelStatus read(uint8_t aAddress, T& aData) – шаблон метода для чтения регистра произвольного типа T по адресу aAdress.

Пример использования:

uint16_t pos; device.read(DYN_ADDRESS_CURRENT_POSITION, pos);

После успешного выполнения команды в переменной pos окажется значение 16-ти разрядного регистра DYN_ADDRESS_CURRENT_POSITION, который занимает в памяти сервопривода 2 байта.

Методы для обмена данными:

DynamixelStatus read(uint8_t aAddress, T& aData) – чтение регистра типа T по адресу aAddress с записью результата в переменную aData.
DynamixelStatus read(uint8_t aAddress, uint8_t size, uint8_t *ptr)– чтение массива из регистров, в количестве size по адресу aAddress с записью результата в массив по адресу ptr.
DynamixelStatus write(uint8_t aAddress, const T& aData) – запись в регистр типа T по адресу aAddress значения aData.
DynamixelStatus write(uint8_t aAddress, uint8_t size, const uint8_t *ptr)- запись в массив регистров, длиной size, по адресу aAddress значений из локального массива с указателем ptr.
DynamixelStatus regWrite(uint8_t aAddress, const T& aData) – команда отложенной записи в регистр типа T по адресу aAddress значения aData. Значение передается на устройство, но применяется к регистру только после команды action().
DynamixelStatus regWrite(uint8_t aAddress, uint8_t size, const uint8_t

*ptr) - команда отложенной записи в массив регистров, длиной size, по адресу aAddress значений из локального массива по указателю ptr.

DynamixelStatus action() – команда, получив которую все устройства в сети применяют значения регистров, полученные с командой отложенной записи. Функция применяется для организации синхронной работы сервоприводов: вначале всем сервоприводам рассылаются целевые значения (положение, скорость), а затем широковещательной командой «action» приводы запускаются.
DynamixelStatus ping() – команда для проверки наличия устройства в сети. В случае, если устройство найдено, возвращает значение «0», иначе – код ошибки.

Примечание. Для отправки широковещательных команд необходимо создать абстрактное устройство DynamixelDevice с широковещательным адресом.

Пример низкоуровнего управления сервоприводом по протоколу Dynamixel 2.0:

#include “DxlMaster.h”

// Добавление библиотеки 
DynamixelDevice device(1);

// Создание объекта управления с адресом 
id = 1 

void setup() {

DxlMaster.begin(1000000);

// Инициализация шины со скоростью 1000000 бод device.init(); 
// Поиск в сети и инициализация устройства 
device.write(DYN_ADDRESS_ENABLE_TORQUE, 1);

// Включение момента 
uint16_t zero_limit = 0;

device.write(DYN_ADDRESS_CW_LIMIT, zero_limit);

// Очистка ограничения вращения по часовой стрелке 
device.write(DYN_ADDRESS_CCW_LIMIT, zero_limit);

// Очистка ограничения вращения против часовой стрелки

}

void loop() {

int16_t speed = 1000; device.write(DYN_ADDRESS_GOAL_SPEED, speed);

// Установка скорости

}

Подключение сервоприводов, класс DynamixelMotor

Для более удобного управления сервоприводами, работающими по протоколу Dynamixel, необходимо применять ориентированный на такое использование класс DynamixelMotor, наследующий класс DynamixelDevice вместе со всеми представленными в предыдущем разделе методами. Дополнительно к методам класса DynamixelDevice в классе DynamixelMotor реализованы следующие специальные методы.

void wheelMode() – настраивает регистры привода для работы в режиме колеса.
void jointMode(uint16_t aCWLimit=0, uint16_t aCCWLimit=0x3FF) – настраивает сервопривод для управления по положению (режим шарнирного сочленения) с ограничениями углов поворота по и против часовой стрелки (aCWLimit и aCCWLimit, соответственно).
void enableTorque(bool aTorque=true) – включает момент, регистр целевого положения принимает значение текущего положения, и сервопривод удерживает текущее положение.
void speed(int16_t aSpeed) – задает скорость вращения сервопривода в режиме колеса, либо максимальную скорость вращения в режиме сочленения.
void goalPosition(uint16_t aPosition) – задает целевое положение при управлении по положению.
void led(uint8_t aState) – управляет светодиодом сервопривода: aState = 0 – выключение, aState = 1 – включение.
uint16_t currentPosition() – опрашивает и возвращает показания о текущем угловом положении сервопривода.
DynamixelStatus getCurrentPosition(uint16_t &pos) – метод аналогичен предыдущему, но возвращает статус выполнения команды для построения более надежных систем управления.

Пример управления сервоприводом по протоколу Dynamixel 1.0:

#include ”DxlMaster.h” // Добавление библиотеки 
DynamixelMotor motor(1);

// Создание объекта класса сервопривода с адресом 1 

void setup() {

DxlMaster.begin(1000000);

// Инициализация шины Dynamixel со скоростью 1000000 бод 
motor.enableTorque(); 
// Включение момента привода 
motor.jointMode(204, 820);

// Включение режима сочленения с угловыми ограничениями 
motor.speed(100);

// Задание максимальной скорости вращения сервопривода

}

void loop() {

motor.goalPosition(123); // Задание положения сервопривода

}

Пример управления сервоприводом по протоколу Dynamixel 2.0:

#include “DxlMaster2.h“ 
// Добавление библиотеки 
DynamixelMotor motor(1);

// Создание объекта класса сервопривода с адресом 1 
void setup() {

DxlMaster.begin(1000000);

// Инициализация шины Dynamixel со скоростью 1000000 бод 
motor.protocolVersion(2.0); // выбор протокола обмена данными 
motor.enableTorque(1); // Включение момента привода 
motor.write(DYN2_ADDR_OPERATION_MODE, (uint8_t)3);

// установка режима работы сервопривода в качестве шарнира = 3 
motor.speed(100);

// Задание максимальной скорости вращения сервопривода

}

void loop() { 

delay(1000);

motor.goalPosition(4000); // Задание положения сервопривода

}

Более подробно возможности библиотеки представлены в примерах среды Arduino IDE (Меню «Файл» -> «Примеры» -> «DxlMaster» или «DxlMaster2»).

Работа модуля в качестве Dynamixel - совместимого устройства, библиотеки DxlSlave и DxlSlave2

Модуль подключается в сеть устройств Dynamixel в качестве ведомого, отвечает на запросы по протоколам Dynamixel 1.0 и Dynamixel 2.0 и передает данные на управляющий контроллер более высокого уровня через интерфейс на основе последовательного порта UART в режиме полудуплекса (Рис.1.1, позиция 3).

Для работы интерфейса и предоставления доступа к данным для опроса по протоколам Dynamixel 1.0 и 2.0 необходимо использовать библиотеки «DxlSlave» и «DxlSlave2.0», которые можно загрузить с сайта компании ООО «Прикладная робототехника» (из раздела «Учебные материалы»). Библиотеки поставляются в виде zip-библиотек Arduino и устанавливаются штатными средствами Arduino IDE (Меню «Скетч» -> Подключить библиотеку -> Добавить .ZIP библиотеку).

Библиотека содержит следующие примеры (меню «Файл» -> Примеры):

allRegs – организация Dynamixel-совместимого устройства с произвольным набором данных.
setEepromDefault – очистка энергонезависимой памяти, используемой для хранения значений регистров Dynamixel.
ax12a (только в DxlSlave) – эмуляция сервопривода ROBOTIS DYNAMIXEL AX-12A.
xl320 (только в DxlSlave2) – эмуляция сервопривода ROBOTIS DYNAMIXEL XL-320.

Область	Адрес (HEX)	Имя	Описание	Доступ	Начальное значение
	0 (0x00)	Model Number(L)	Младший байт номера модели устройства	R*	12 (0x0C)
	1 (0x01)	Model Number(H)	Старший байт номера модели устройства	R	0 (0x00)
	2 (0x02)	Version of Firmware	Версия программного обеспечения устройства	R	-
	3 (0x03)	ID	Адрес устройства в сети Dynamixel	RW	1 (0x01)
	4 (0x04)	Baud Rate	Скорость обмена данными (делитель). 1 – 1000000 бод	RW	1 (0x01)
	5 (0x05)	Return Delay Time	Задержка ответа на запросы для повышения надежности передачи данных	RW	250 (0xFA)
EEPROM	6 (0x06)		Произвольные данные	RW	0(0x00)
	…	...	...	...	...
	15 (0x0F)		Произвольные данные	RW	0(0x00)
	16 (0x10)	Status Return Level	Настройка протокола: будет ли устройство отвечать на запросы	RW	2 (0x02)
	17 (0x11)		Произвольные данные	RW	0(0x00)
	…	...	...	...	...
	23 (0x17)		Произвольные данные	RW	0(0x00)
	24 (0x18)		Произвольные данные	RW	0(0x00)
RAM	…	...	...	...	...
	255 (0xFF)		Произвольные данные	RW	0(0x00)

Таблица 1. Назначение регистров Dynamixel

R – внешним устройствам доступно только чтение регистра, RW – чтение и запись.

Стандартная организация адресного пространства Dynamixel

В библиотеках организовано регистровое управление, аналогичное устройствам Dynamixel – данные, доступные внешним устройствам, необходимо хранить в выделенной области памяти библиотеки в 255 байт (регистров), каждый из которых имеет адрес от 0 до 255.

Некоторые регистры по стандартным адресам должны содержать типовые для Dynamixel-совместимых устройств значения и заполняются библиотекой автоматически, другие отведены для пользовательских задач. Адресное пространство и назначение регистров приведено в таблице 1.

Инициализация библиотеки

Во всех случаях использования библиотеку необходимо добавить в программу и инициализировать. Для добавления библиотеки необходимо подключить заголовочный файл в начале программы.

Для работы с протоколом Dynamixel 1.0:

#include “DxlSlave.h“

Для работы с протоколом Dynamixel 2.0:

#include “DxlSlave2.h“

При добавлении заголовочного файла библиотеки становится доступен глобальный объект DxlSlave (одинаковый для любой версии протокола), который необходимо инициализировать следующим образом:

DxlSlave.begin(uint16_t model_number, uint8_t fw_version);

где model_number – номер модели устройства и fw_version – версия программного обеспечения, которые будут показаны внешним устройствам.

Работа с интерфейсом, класс DxlSlave

После инициализации библиотека работает в фоновом режиме и автоматически отвечает на запросы внешних устройств, отправляя данные из регистров или устанавливая их новые значения. Изменение значений системных (стандартных) регистров с настройками параметров передачи данных (ID устройства, baud rate, return delay time, status return level) автоматически приводит к изменению этих параметров. Изменение значений

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

необходимо записать, данные для передачи управляющему устройству, в регистр. Они автоматически будут отправлены управляющему устройству по запросу;
проверить наличие необработанных изменений в очереди (polling), для того чтобы узнать об изменениях регистров, выполненных управляющим устройством, и произвести соответствующие действия.
чтобы узнать об изменениях регистров, выполненных управляющим устройством, и произвести соответствующие действия, необходимо проверить наличие необработанных изменений в очереди (polling).

Для работы с интерфейсом необходимо использовать класс DxlSlave, предоставляющий следующие методы.

void begin(uint16_t model_number, uint8_t fw_version) - инициализация библиотеки. Значения model_number и fw_version записываются в регистры Model Number и Version of Firmware в соответствии с таблицей стандартных регистров. После вызова метода, устройство начинает отвечать и автоматически (в фоновом режиме) реагировать на запросы Dynamixel.
void set_id(uint8_t id) - записывает ID в соответствии с таблицей регистров. После этого устройство автоматически меняет адрес доступа.
void set_baud(uint8_t baud) - записывает Baud Rate в соответствии с таблицей регистров. После этого устройство меняет скорость передачи данных (baud rate).
void set(uint8_t addr, uint8_t val) - записывает значение val в регистр по адресу addr. Значение не перезаписывается в EEPROM, если равно уже записанному. При изменении значений регистров, отвечающих за настройки протокола, автоматически меняются соответствующие параметры передачи данных.
uint8_t get(uint8_t addr) - возвращает значение, записанное в регистре по адресу addr.
void set_mode(uint8_t addr, uint8_t mode) - настраивает режим доступа mode (0 – R (чтение), 1 - RW (чтение и запись)) к регистру по адресу addr.
void set_mode16(uint8_t addr, uint8_t mode) - настраивает режим доступа к 2-м регистрам подряд начиная с адреса addr.
void set16(uint8_t addr, uint16_t val) - записывает 16-ти разрядное значение в 2 регистра, расположенных по адресу addr. Последовательность байт: младший байт первый. Значение не должно перезаписываться в EEPROM, если равно уже записанному.
uint16_t get16(uint8_t addr) - возвращает 16-ти разрядное значение, записанное в 2-х регистрах (первый байт младший).
uint8_t poll()- возвращает 1, если со стороны ведущего устройства через протокол Dynamixel было изменено значение какого-либо регистра (очередь измененных регистров не пуста); 0, если изменений нет (очередь пуста).
uint8_t scan() - возвращает адрес первого в очереди (кольцевом буфере) измененного через протокол Dynamixel регистра и удаляет его из очереди (смещает указатель кольцевого буфера на следующий измененный регистр).

Примеры работы с библиотеками DxlSlave и DxlSlave2

В следующем примере показана организация Dynamixel-совместимого устройства с произвольным набором данных. В этом примере при изменении ведущим устройством регистра разрабатываемого ведомого устройства, ведомое устройство выводит в терминал адрес и значение измененного регистра.

#include <DxlSlave.h> // Подключение библиотеки

#define IS_INITED_REG 23 // Адрес регистра для проверки 1-го запуска 

void setup() {

DxlSlave.begin(1234, 12); // Инициализация интерфейса (номер модели 1234, версия 12)

Serial.begin(9600); // Инициализация отладочного последовательного порта для вывода

if(!DxlSlave.get(IS_INITED_REG)) { // Инициализация EEPROM при 1-м запуске

for(int i = 6; i < 23; i++) {

DxlSlave.set(i, i); // Заполнение регистров EEPROM произвольными значениями

DxlSlave.set(IS_INITED_REG, 1); // Установка флага “1-я инициализация выполнена“

}

}

for(int i = 24; i < 255; i++) {

DxlSlave.set(i, i); // Заполнение регистров RAM произвольными значениями

}

}

void loop() {

while(DxlSlave.poll()) { // Проверка наличия изменений регистров ведущим устройством

uint8_t addr = DxlSlave.scan(); // Получение адреса измененного регистра из очереди

Serial.print(“ The value at address “); // Вывод информации 
Serial.print(addr);

Serial.print(“ was changed by master. New value is “); Serial.println(DxlSlave.get(addr));

}

}

Подробнее о возможностях библиотеки представлены в примерах (Меню «Файл» -> Примеры -> DxlSlave).

Для очистки EEPROM устройства, в том числе, для очистки флага и вызова 1-й инициализации EEPROM при изменении программы, необходимо использовать пример setEepromDefault.

Организация произвольного Dynamixel-совместимого устройства с использованием более разнообразных команд приведена в стандартном примере allRegs библиотеки.

Возможности библиотеки DxlSlave продемонстрированы также в примере «ax12a», где эмулируется сервопривод ROBOTIS DYNAMIXEL AX-12, распознаваемый стандартными утилитами ROBOTIS.

Аналогичный пример эмуляции сервопривода XL-320 с протоколом Dynamixel 2.0 приведен в библиотеке DxlSlave2.