Глава 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:
cpp
#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:
cpp
#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:
cpp
#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-совместимого устройства с произвольным набором данных. В этом примере при изменении ведущим устройством регистра разрабатываемого ведомого устройства, ведомое устройство выводит в терминал адрес и значение измененного регистра.
cpp
#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.