Глава 5. Программирование и отладка
Подготовка к программированию
Для работы с контроллером OpenCM9.04 будет использоваться стандартная среда программирования Arduino IDE. Однако, для данного контроллера понадобится установить в среду некоторые дополнительные компоненты и библиотеки.
Рис. 5.1. Открытие настроек среды Arduino IDE
Во-первых, необходимо установить поддержку плат OpenCM9.04. Для этого нужно:
- Указать дополнительную загрузочную ссылку для менеджера плат. Это можно сделать, выбрав в меню программы «Файл» / «Настройки» (рис. 5.1.) И прописать в строке «Дополнительные ссылки для менеджера плат» следующее (рис. 5.2):
https://raw.githubusercontent.com/ ROBOTIS-GIT/OpenCM9.04/master/ arduino/opencm_release/ package_ opencm9.04_index.json
Затем нажать ОК.
Рис. 5.2. Добавление ссылки для менеджера плат
- Перейти в менеджер плат, выбрав в меню «Инструменты» / «Менеджер плат…» (рис. 5.3):
Рис. 5.3. Меню менеджера плат
В поисковом окне ввести «opencm» и установить (install) последнюю версию указанного ниже пакета (в данном случае версию 1.4.1). После установки пакета рядом с его названием и версией появится надпись
«INSTALLED», как показано на рисунке 5.4:
Рис. 5.4. Установка пакета OpenCM
- Кроме того, необходимо скачать библиотеку для сервоприводов AR-S430-01. Актуальные библиотеки можно найти на сайте: https://github. com/AppliedRobotics/STEM_examples/ Актуальные библиотеки можно найти на сайте: https://github.com/AppliedRobotics/STEM_examples/
Далее необходимо добавить библиотеку (меню «Скетч» / «Подключить библиотеку» / «Добавить .ZIP библиотеку…») (рис. 5.5.):
В появившемся окне открыть загруженную библиотеку. (рис. 5.6):
Рис. 5.5. Меню управления библиотеками
После установки компонентов необходимо в менеджере плат найти и выбрать используемую нами плату («OpenCM9.04 Board») (рис. 5.7):
Рис. 5.6. Установка библиотеки dynamixel2arduino
Рис. 5.7. Выбор необходимой платы для работы
После установки всех вышеописанных компонентов и библиотек контроллер OpenCM9.04 можно подключить с помощью провода USBmicroUSB к компьютеру. После подключения контроллера нужно выбрать COM-порт, к которому была подключена плата.
Сама плата поставляется в комплекте с периферийной платой STEM Board, расширяющей возможности контроллера OpenCM9.04.
Изучение оборудования
На изображениях ниже (рис. 5.8 - 5.12) представлено оборудование, задействованное на этапе программирования.
Рис. 5.8. OpenCM9.04 Рис. 5.9. STEM Board
Рис. 5.10. Блок питания
Рис. 5.11. Кабель USB-microUSB
Рис. 5.12. Собранный манипулятор
Далее более подробно рассмотрены каждая из плат и выделены их основные компоненты.
Контроллер OpenCM9.04
Рис. 5.13. Вид контроллера OpenCM9.04 сверху. Красным выделен разъем microUSB, желтым – кнопка Reset, синим – пользовательская кнопка загрузки UserSW (в будущем, при возникновении проблем с загрузкой кода, необходимо зажать Reset+SW, чтобы перевести плату в режим загрузки.
Рис. 5.14. Вид контроллера OpenCM9.04 снизу. Красным выделен управляющий микроконтроллер.
Периферийная плата STEM Board
Рис. 5.15. Вид платы STEM Board сверху. Красным выделен разъем силового питания, желтым – тумблер силового питания, зеленым – разъем для подключения манипулятора.
Для начала работы необходимо соединить контроллер OpenCM9.04 и компьютер, а также подключить кабель блока питания в разъем силового питания периферийной платы STEM Board.
Внимание! Во время подключения силового питания тумблер силового питания должен находиться в положении OFF!!! В этом же положении он должен находиться большую часть работы, и включаться только тогда, когда идет тестирование написанной программы на манипуляторе. Также необходимо соединить STEM Board и манипулятор.
После выполнения всех действий, перечисленных выше, программируемый контроллер и манипулятор готовы к работе.
Перед началом написания управляющих программ стоит вспомнить принципы управления сервоприводами, которые используются в наборе. Сервоприводы могут функционировать в двух режимах: в режиме колеса (их можно установить в качестве колес мобильных роботов и управлять их угловой скоростью), и в режиме сустава (или шарнира; приводы можно установить для отработки заданного угла, управление идет и по углу, и по скорости). В нашем случае, мы будем использовать режим сустава. Данные сервоприводы могут поворачиваться на угол от 0° до 360°. При этом на двигатели будет отправляться специальная команда с обозначением угла, закодированным пропорционально его величине. Нулю градусам соответствует 0 условных единиц, а 360 градусам соответствует 16384 условные единицы. При этом, среднему положению 180° = 8192 условных единиц соответствует совпадение единичных рисок на корпусе сервопривода и его выходном валу.
5.2.3. Схема подключения OpenCM и NanoPi-AR к STEM Board
Рис. 5.16. Схема подключения
Начало программирования
В качестве первой программы для усвоения принципов программирования с имеющимся контроллером предлагается рассмотреть классическую программу мигания светодиодом на плате, подключение манипулятора или блока питания для этого не понадобится.
Мигание диодом
Программа мигания диодом приведена далее. Первая версия программы – упрощенная (diode_1).
int led_pin = 14; // инициализация переменной, отвечающей за номер пина, подключенного к диоду
void setup() { // структура, команды внутри которой выполняются единожды
// Set up the built-in LED pin as an output:
// установка пина встроенного светодиода на выход
pinMode(led_pin, OUTPUT); // настройка режима работы диодного пина в качестве выхода
}
void loop() { // структура, команды внутри которой выполняются многократно
digitalWrite(led_pin, HIGH); // отправка на выход сигнала высокого уровня, диод гаснет
delay(1000); // задержка в 1 секунду
digitalWrite(led_pin, LOW); // отправка на выход сигнала низкого уровня, диод загорается
delay(1000); // задержка в 1 секунду
}
Во второй версии программы подключается последовательный порт (идущий к компьютеру) для отправки микроконтроллером сообщений на него – для информирования (diode_2).
int led_pin = 14; // инициализация переменной, отвечающей за номер пина, подключенного к диоду
void setup() { // структура, команды внутри которой выполняются единожды
// Set up the built-in LED pin as an output: // установка пина встроенного светодиода на выход
pinMode(led_pin, OUTPUT); // настройка режима работы диодного пина в качестве выхода
Serial.begin(57600); // установка скорости обмена данными по последовательному порту
}
void loop() { // структура, команды внутри которой выполняются многократно
digitalWrite(led_pin, HIGH); // отправка на выход сигнала высокого уровня, диод гаснет
Serial.println(«led_off»); // отправка в монитор порта текста led_off
(диод гаснет)
delay(1000); // задержка в 1 секунду
digitalWrite(led_pin, LOW); // отправка на выход сигнала низкого уровня, диод загорается
Serial.println(«led_on»); // отправка в монитор порта текста led_on (диод горит)
delay(100); // задержка в 1 секунду
}
Вращение сервопривода
Далее логически можно перейти к вращению сервоприводов, так как команды для перемещения сервопривода подаются аналогично командам, зажигающим и гасящим диод.
Предположим, что необходимо, чтобы сервопривод поворачивался на угол +90 и -90 от текущего положения (то есть в сумме на 180 ).
Для этого найдем соответствующие этим углам значения в условных единицах. Одному градусу соответствует 45,5 условные единицы:
\(360 = 16383\)
\(1 = 16383 / 360 = 45,5\)
Таким образом, если текущее положение = 0:
\(90 = 45,5 * 90 = 4095\)
Полученное значение условных единиц угла поворота сервопривода необходимо округлить, так как в командах можно указывать только целое число.
Для управления сервоприводом и задания поворота на требуемый угол используется 2 команды.
Рекомендуется не задавать скорость выше 100, рекомендуемое значение – 50, в ином случае манипулятор при движении может сломаться, а также повредить окружающих.
Вторая команда задает угол:
//dxl.setGoalPosition(joint, 4095);
Например, эта команда для сервопривода с ID=joint повернуться на угол 90 градусов.
#include <Dynamixel2Arduino.h> // подключение библиотеки Dynamixel
// Последовательный порт DXL для платы STEM с OpenCM 9.04. (Если использовать DXL порт
// на самой плате OpenCM 9.04, то нужно использовать Serial1, и из-за кода в драйвере,
// необходимо перед dxl.begin() вызвать Serial1.setDxlMode(true).)
#define DXL_SERIAL Serial3
#define DEBUG_SERIAL Serial // последовательный порт, подключаемый к компьютеру
const uint8_t DXL_DIR_PIN = 22; // номер информационного пина сервоприводов (28 для OpenCM 9.04)
const float DXL_PROTOCOL_VERSION = 2.0; // протокол передачи данных от OpenCM9.04 к сервоприводам
Dynamixel2Arduino dxl(DXL_SERIAL, DXL_DIR_PIN); // инициализация указателя на команды из библиотеки Dynamixel
int joint=4; // инициализация переменной, отвечающей за номер управляемого сервопривода
void setup() { // функция, внутри которой команды выполняются единожды
DEBUG_SERIAL.begin(57600); // установка скорости обмена данными по последовательному порту компьютера
dxl.begin(1000000); // установка скорости обмена данными по последовательному порту манипулятора
dxl.setPortProtocolVersion(DXL_PROTOCOL_VERSION); // выбор протокола обмена данными
dxl.torqueOff(joint); // отключаем крутящий момент, когда изменяем EEPROM область в сервоприводе
dxl.setOperatingMode(joint, OP_POSITION); // установка режима работы сервопривода с номером joint в качестве шарнира
}
void loop() { // функция, внутри которой команды выполняются многократно
dxl.torqueOn(joint); // включаем крутящий момент сервопривода для удержания целевого положения
dxl.setGoalVelocity(joint, 100); // задание целевой скорости 100 сервоприводу с номером joint
dxl.setGoalPosition(joint, 4095); // задание целевого положения 90 градусов сервоприводу с номером joint
delay(5000); // задержка в 5 секунд, ожидание, пока сервопривод займет нужное положение
dxl.setGoalVelocity(joint, 100); // задание целевой скорости 100 сервоприводу с номером joint
dxl.setGoalPosition(joint, 0); // задание целевого положения 0 градусов сервоприводу с номером joint
delay(5000); // задержка в 5 секунд, ожидание, пока сервопривод займет нужное положение
}
Вращение всех сервоприводов
Рассмотрим программу, которая управляла бы всеми сервоприводами рассматриваемого манипулятора одновременно. Очевидно, что для этого команды задания скорости и положения необходимо продублировать 5 раз.
#include <Dynamixel2Arduino.h> // подключение библиотеки Dynamixel
// Последовательный порт DXL для платы STEM с OpenCM 9.04. (Если использовать DXL порт
// на самой плате OpenCM 9.04, то нужно использовать Serial1, и из-за кода в драйвере,
// необходимо перед dxl.begin() вызвать Serial1.setDxlMode(true).)
#define DXL_SERIAL Serial3
#define DEBUG_SERIAL Serial // последовательный порт, подключаемый к компьютеру
#define DEGREE_COEFF 16383/360 // 0...16383 = 0...360 градусов для сервоприводов
const uint8_t DXL_DIR_PIN = 22; // номер информационного пина сервоприводов (28 для OpenCM 9.04)
const float DXL_PROTOCOL_VERSION = 2.0; // протокол передачи данных от OpenCM9.04 к сервоприводам
Dynamixel2Arduino dxl(DXL_SERIAL, DXL_DIR_PIN); // инициализация указателя на команды из библиотеки Dynamixel
void setup() { // функция, внутри которой команды выполняются единожды
DEBUG_SERIAL.begin(57600); // установка скорости обмена данными по последовательному порту компьютера
dxl.begin(1000000); // установка скорости обмена данными по последовательному порту манипулятора
dxl.setPortProtocolVersion(DXL_PROTOCOL_VERSION); // выбор протокола обмена данными
dxl.torqueOff(1); // отключаем крутящий момент, когда изменяем EEPROM область в сервоприводе
dxl.torqueOff(2);
dxl.torqueOff(3);
dxl.torqueOff(4);
dxl.torqueOff(5);
dxl.setOperatingMode(1, OP_POSITION); // установка режима работы сервопривода с номером 1 в качестве шарнира
dxl.setOperatingMode(2, OP_POSITION);
dxl.setOperatingMode(3, OP_POSITION);
dxl.setOperatingMode(4, OP_POSITION);
dxl.setOperatingMode(5, OP_POSITION); // установка режима работы сервопривода с номером 5 в качестве шарнира
dxl.torqueOn(1); // включаем крутящий момент сервопривода для удержания целевого положения
dxl.torqueOn(2);
dxl.torqueOn(3);
dxl.torqueOn(4);
dxl.torqueOn(5);
}
void loop() { // функция, внутри которой команды выполняются многократно
dxl.setGoalVelocity(1, 50); // задание целевой скорости 50 сервоприводу с номером 1
dxl.setGoalPosition(1, 30*DEGREE_COEFF); // задание целевого положения 30 градусов сервоприводу с номером 1
dxl.setGoalVelocity(2, 50); // задание целевой скорости 50 сервоприводу с номером 2
dxl.setGoalPosition(2, 95*DEGREE_COEFF); // задание целевого положения -5 градусов от 100 сервоприводу с номером 2
dxl.setGoalVelocity(3, 50); // задание целевой скорости 50 сервоприводу с номером 3
dxl.setGoalPosition(3, 250*DEGREE_COEFF); // задание целевого положения +10 градусов от 240 сервоприводу с номером 3
dxl.setGoalVelocity(4, 50); // задание целевой скорости 50 сервоприводу с номером 4
dxl.setGoalPosition(4, 30*DEGREE_COEFF); // задание целевого положения +30 градусов сервоприводу с номером 4
dxl.setGoalVelocity(5, 50); // задание целевой скорости 50 сервоприводу с номером 5
dxl.setGoalPosition(5, 242*DEGREE_COEFF); // задание целевого положения +10 градусов сервоприводу с номером 5
delay(5000); // задержка в 5 секунд, ожидание, пока сервоприводы займут нужное положение
dxl.setGoalVelocity(1, 50); // задание целевой скорости 50 сервоприводу с номером 1
dxl.setGoalPosition(1, 0); // задание целевого положения 0 градусов сервоприводу с номером 1
dxl.setGoalVelocity(2, 50); // задание целевой скорости 50 сервоприводу с номером 2
dxl.setGoalPosition(2, 100*DEGREE_COEFF); // задание целевого положения 100 градусов сервоприводу с номером 2
dxl.setGoalVelocity(3, 50); // задание целевой скорости 50 сервоприводу с номером 3
dxl.setGoalPosition(3, 240*DEGREE_COEFF); // задание целевого положения 240 градусов сервоприводу с номером 3
dxl.setGoalVelocity(4, 50); // задание целевой скорости 50 сервоприводу с номером 4
dxl.setGoalPosition(4, 0); // задание целевого положения 0 градусов сервоприводу с номером 4
dxl.setGoalVelocity(5, 50); // задание целевой скорости 50 сервоприводу с номером 5
dxl.setGoalPosition(5, 232*DEGREE_COEFF); // задание целевого положения 232 градусов сервоприводу с номером 5
delay(5000); // задержка в 5 секунд, ожидание, пока сервоприводы займут нужное положение
}
Обратите внимание, что при управлении всеми сервоприводами одновременно уже нельзя задавать углы ±90°, так как в этом случае манипулятор будет стремиться занять недопустимую конфигурацию, упираясь сам в себя, что может привести к поломке. На этот случай во время испытаний всегда стоит держать плату управления под рукой, чтобы переключить тумблер силового питания. Углы вращения 5 сервопривода также конструктивно ограничены, что необходимо учитывать при написании управляющих программ манипулятора.
При вращении всеми сервоприводами одновременно будем отклоняться на угол ±30° от среднего положения.
После написания программы с движением всех сервоприводов одновременно можно рассмотреть программу с движением всех сервоприводов по отдельности. Изменится программа не сильно.
#include <Dynamixel2Arduino.h> // подключение библиотеки Dynamixel
// Последовательный порт DXL для платы STEM с OpenCM 9.04. (Если использовать DXL порт на самой плате OpenCM 9.04, то нужно использовать Serial1, и из-за кода в драйвере, необходимо перед dxl.begin() вызвать Serial1.setDxlMode(true).)
#define DXL_SERIAL Serial3
#define DEBUG_SERIAL Serial // последовательный порт, подключаемый к компьютеру
#define DEGREE_COEFF 16383/360 // 0...16383 = 0...360 градусов для сервоприводов
const uint8_t DXL_DIR_PIN = 22; // номер информационного пина сервоприводов (28 для OpenCM 9.04)
const float DXL_PROTOCOL_VERSION = 2.0; // протокол передачи данных от OpenCM9.04 к сервоприводам
Dynamixel2Arduino dxl(DXL_SERIAL, DXL_DIR_PIN); // инициализация указателя на команды из библиотеки Dynamixel
void setup() { // функция, внутри которой команды выполняются единожды
DEBUG_SERIAL.begin(57600); // установка скорости обмена данными по последовательному порту компьютера
dxl.begin(1000000); // установка скорости обмена данными по последовательному порту манипулятора
dxl.setPortProtocolVersion(DXL_PROTOCOL_VERSION); // выбор протокола обмена данными
dxl.torqueOff(1); // отключаем крутящий момент, когда изменяем EEPROM область в сервоприводе
dxl.torqueOff(2);
dxl.torqueOff(3);
dxl.torqueOff(4);
dxl.torqueOff(5);
dxl.setOperatingMode(1, OP_POSITION); // установка режима работы сервопривода с номером 1 в качестве шарнира dxl.setOperatingMode(2, OP_POSITION);
dxl.setOperatingMode(3, OP_POSITION);
dxl.setOperatingMode(4, OP_POSITION);
dxl.setOperatingMode(5, OP_POSITION); // установка режима работы сервопривода с номером 5 в качестве шарнира
dxl.torqueOn(1); // включаем крутящий момент сервопривода для удержания целевого положения
dxl.torqueOn(2);
dxl.torqueOn(3);
dxl.torqueOn(4);
dxl.torqueOn(5);
}
void loop() { // функция, внутри которой команды выполняются многократно
dxl.setGoalVelocity(1, 50); // задание целевой скорости 50 сервоприводу с номером 1
dxl.setGoalPosition(1, 30*DEGREE_COEFF); // задание целевого положения 30 градусов сервоприводу с номером 1
delay(5000); // задержка в 5 секунд, ожидание, пока сервоприводы займут нужное положение
dxl.setGoalVelocity(2, 50); // задание целевой скорости 50 сервоприводу с номером 2
dxl.setGoalPosition(2, 95*DEGREE_COEFF); // задание целевого положения -5 градусов от 100 сервоприводу с номером 2
delay(5000); // задержка в 5 секунд, ожидание, пока сервоприводы займут нужное положение
dxl.setGoalVelocity(3, 50); // задание целевой скорости 50 сервоприводу с номером 3
dxl.setGoalPosition(3, 250*DEGREE_COEFF); // задание целевого положения +10 градусов от 240 сервоприводу с номером 3
delay(5000); // задержка в 5 секунд, ожидание, пока сервоприводы займут нужное положение
dxl.setGoalVelocity(4, 50); // задание целевой скорости 50 сервоприводу с номером 4
dxl.setGoalPosition(4, 30*DEGREE_COEFF); // задание целевого положения +30 градусов сервоприводу с номером 4
delay(5000); // задержка в 5 секунд, ожидание, пока сервоприводы займут нужное положение
dxl.setGoalVelocity(5, 50); // задание целевой скорости 50 сервоприводу с номером 5
dxl.setGoalPosition(5, 242*DEGREE_COEFF); // задание целевого положения +10 градусов сервоприводу с номером 5
delay(5000); // задержка в 5 секунд, ожидание, пока сервоприводы займут нужное положение
dxl.setGoalVelocity(1, 50); // задание целевой скорости 50 сервоприводу с номером 1
dxl.setGoalPosition(1, 0); // задание целевого положения 0 градусов сервоприводу с номером 1
delay(5000); // задержка в 5 секунд, ожидание, пока сервоприводы займут нужное положение
dxl.setGoalVelocity(2, 50); // задание целевой скорости 50 сервоприводу с номером 2
dxl.setGoalPosition(2, 100*DEGREE_COEFF); // задание целевого положения 100 градусов сервоприводу с номером 2
delay(5000); // задержка в 5 секунд, ожидание, пока сервоприводы займут нужное положение
dxl.setGoalVelocity(3, 50); // задание целевой скорости 50 сервоприводу с номером 3
dxl.setGoalPosition(3, 240*DEGREE_COEFF); // задание целевого положения 240 градусов сервоприводу с номером 3
delay(5000); // задержка в 5 секунд, ожидание, пока сервоприводы займут нужное положение
dxl.setGoalVelocity(4, 50); // задание целевой скорости 50 сервоприводу с номером 4
dxl.setGoalPosition(4, 0); // задание целевого положения 0 градусов сервоприводу с номером 4
delay(5000); // задержка в 5 секунд, ожидание, пока сервоприводы займут нужное положение
dxl.setGoalVelocity(5, 50); // задание целевой скорости 50 сервоприводу с номером 5
dxl.setGoalPosition(5, 232*DEGREE_COEFF); // задание целевого положения 232 градусов сервоприводу с номером 5
delay(5000); // задержка в 5 секунд, ожидание, пока сервоприводы займут нужное положение
}
Чтение позиций сервоприводов
Следующей рассматриваемой программой является управляющая программа контроллера для считывания позиции сервопривода. Так же, как и в ранних примерах, будет считываться положение 4 привода.
Обратите внимание, что во время считывания позиции сервопривода он автоматически блокируется. Ввиду этого не получится двигать манипулятор и перемещать сервопривода для отслеживания изменения позиции. Поэтому для отключения блокировки будет использоваться специальная команда.
#include <Dynamixel2Arduino.h> // подключение библиотеки Dynamixel
// Последовательный порт DXL для платы STEM с OpenCM 9.04. (Если использовать DXL порт
// на самой плате OpenCM 9.04, то нужно использовать Serial1, и из-за кода в драйвере,
// необходимо перед dxl.begin() вызвать Serial1.setDxlMode(true).)
#define DXL_SERIAL Serial3
#define DEBUG_SERIAL Serial // последовательный порт, подключаемый к компьютеру
const uint8_t DXL_DIR_PIN = 22; // номер информационного пина сервоприводов (28 для OpenCM 9.04)
const float DXL_PROTOCOL_VERSION = 2.0; // протокол передачи данных от OpenCM9.04 к сервоприводам
Dynamixel2Arduino dxl(DXL_SERIAL, DXL_DIR_PIN); // инициализация указателя на команды из библиотеки Dynamixel
int pos = 0; // переменная для положения сервопривода
void setup() { // функция, внутри которой команды выполняются единожды
DEBUG_SERIAL.begin(57600); // установка скорости обмена данными по последовательному порту компьютера
dxl.begin(1000000); // установка скорости обмена данными по последовательному порту манипулятора
dxl.setPortProtocolVersion(DXL_PROTOCOL_VERSION); // выбор протокола обмена данными
dxl.torqueOff(4); // отключаем крутящий момент, когда изменяем EEPROM область в сервоприводе
dxl.setOperatingMode(4, OP_POSITION); // установка режима работы сервопривода с номером 1 в качестве шарнира
}
void loop() {
pos = dxl.getPresentPosition(4); // получение текущей позиции сервопривода с номером 4 и запись в переменную pos
dxl.torqueOff(4); // отключение блокировки сервопривода с номером 4
Serial.println(pos); // вывод в монитор порта значения текущего положения 4 сервопривода
delay(1000); // задержка, пауза в 1 секунду
}
После написания программы для определения позиции одного сервопривода необходимо также написать программу для определения позиции сразу всех сервоприводов (с записью в одномерный массив, поэтому также необходимо вкратце рассказать, что такое массивы. Если переменная — это ячейка, содержащая информацию, то массив – это упорядоченный набор ячеек).
Данные после считывания будут выводиться на экран (в монитор порта), причем, в заданном формате.
#include <Dynamixel2Arduino.h> // подключение библиотеки Dynamixel
// Последовательный порт DXL для платы STEM с OpenCM 9.04. (Если использовать DXL порт
// на самой плате OpenCM 9.04, то нужно использовать Serial1, и из-за кода в драйвере,
// необходимо перед dxl.begin() вызвать Serial1.setDxlMode(true).)
#define DXL_SERIAL Serial3
#define DEBUG_SERIAL Serial // последовательный порт, подключаемый к компьютеру
const uint8_t DXL_DIR_PIN = 22; // номер информационного пина сервоприводов (28 для OpenCM 9.04)
const float DXL_PROTOCOL_VERSION = 2.0; // протокол передачи данных от OpenCM9.04 к сервоприводам
Dynamixel2Arduino dxl(DXL_SERIAL, DXL_DIR_PIN); // инициализация указателя на команды из библиотеки Dynamixel
#define jointN 5 // инициализация константы, обозначающей количество сервоприводов (и, соответственно, элементов массива)
int pos = 0; // инициализация переменной для положения сервопривода
int i = 0; // инициализация переменной счетчика циклов
int buf[jointN+1]; // инициализация одномерного массива, размер его задается 5+1=6, так как в программировании нумерация элементов начинается с 0, но нулевой элемент мы использовать не будем, так как начнем с первого
void setup() {
DEBUG_SERIAL.begin(57600); // установка скорости обмена данными по последовательному порту компьютера
dxl.begin(1000000); // установка скорости обмена данными по последовательному порту манипулятора dxl.setPortProtocolVersion(DXL_PROTOCOL_VERSION); // выбор протокола обмена данными
for (i=1; i<=jointN; i++) // цикл с изменением i от 1 до 5 с шагом 1
{
dxl.torqueOff(i); // отключаем крутящий момент, когда изменяем EEPROM область в сервоприводе
dxl.setOperatingMode(i, OP_POSITION); // установка режима работы сервопривода с номером i в качестве шарнира
}
}
void loop() {
for (i=1; i<=jointN; i++) // цикл с изменением i от 1 до 5 с шагом 1
{
pos = dxl.getPresentPosition(i); // получение текущей позиции сервопривода с номером i и запись в переменную pos
buf[i] = pos; // запись переменной значения pos в i-тый элемент массива buf
dxl.torqueOff(i); // отключение блокировки сервопривода с номером i
}
DEBUG_SERIAL.print("{0, "); // вывод в монитор порта текста "{0, " for (i=1; i<=jointN; i++) // цикл с изменением i от 1 до 5 с шагом 1
{
DEBUG_SERIAL.print(buf[i]);// вывод в монитор порта значения ячейки массива с номером i
DEBUG_SERIAL.print(", ");// вывод в монитор порта текста ", "
}
DEBUG_SERIAL.print("}, ");// вывод в монитор порта текста "}, "
DEBUG_SERIAL.println(‘ ‘);// вывод в монитор порта текста " " и перенос курсора на следующую строку
delay(1000); // задержка, пауза в 1 секунду
}
Цикличное вращение всех сервоприводов
В качестве дальнейшего изучения рекомендуется использовать циклы для упрощения и укорачивания кода управляющих программ. В следующих программах будет использоваться цикл-счетчик for. С помощью данного цикла все сервоприводы будут перебираться последовательно. При использовании циклов будет дополнительно введена переменная-счетчик i, которая, изменяя свое значение, позволит перебирать номера сервоприводов.
В результате, будут получены управляющие программы контроллера, выполняющие те же самые функции, что и предыдущие две программы, но выглядеть они будут намного короче. В качестве примера рассмотрим модификацию первой программы из предыдущего пункта:
#include <Dynamixel2Arduino.h> // подключение библиотеки Dynamixel
// Последовательный порт DXL для платы STEM с OpenCM 9.04. (Если использовать DXL порт
// на самой плате OpenCM 9.04, то нужно использовать Serial1, и изза кода в драйвере, необходимо перед dxl.begin() вызвать Serial1. setDxlMode(true).)
#define DXL_SERIAL Serial3
#define DEBUG_SERIAL Serial // последовательный порт, подключаемый к компьютеру
#define DEGREE_COEFF 16383/360 // 0...16383 = 0...360 градусов для сервоприводов
const uint8_t DXL_DIR_PIN = 22; // номер информационного пина сервоприводов (28 для OpenCM 9.04)
const float DXL_PROTOCOL_VERSION = 2.0; // протокол передачи данных от OpenCM9.04 к сервоприводам
Dynamixel2Arduino dxl(DXL_SERIAL, DXL_DIR_PIN); // инициализация указателя на команды из библиотеки Dynamixel
#define jointN 5 // инициализация константы, обозначающей количество сервоприводов (и, соответственно, элементов массива)
int pos = 0; // инициализация переменной для положения сервопривода
int i = 0; // инициализация переменной счетчика циклов
int buf[jointN+1]; // инициализация одномерного массива, размер его задается 5+1=6, так как в программировании нумерация элементов начинается с 0, но нулевой элемент мы использовать не будем, так как начнем с первого
void setup() { // функция, внутри которой команды выполняются единожды
DEBUG_SERIAL.begin(57600); // установка скорости обмена данными по последовательному порту компьютера
dxl.begin(1000000); // установка скорости обмена данными по последовательному порту манипулятора
dxl.setPortProtocolVersion(DXL_PROTOCOL_VERSION); // выбор протокола обмена данными
for (i=1; i<=jointN; i++) { // цикл с изменением счетчика цикла i от 1 до 5 с шагом 1 (для перебора всех 5 сервоприводов)
dxl.torqueOff(i); // отключаем крутящий момент, когда изменяем EEPROM область в сервоприводе
dxl.setOperatingMode(i, OP_POSITION); // установка режима работы сервопривода с номером i в качестве шарнира
dxl.torqueOn(i); // включаем крутящий момент сервопривода для удержания целевого положения
}
}
void loop() { // функция, внутри которой команды выполняются многократно
for (i=1; i<=jointN; i++) {
pos = dxl.getPresentPosition(i); // получение текущей позиции сервопривода с номером i и запись в переменную pos
buf[i] = pos; // запись переменной значения pos в i-тый элемент массива buf
}
for (i=1; i<=jointN; i++) {
dxl.setGoalVelocity(i, 50); // задание целевой скорости 50 сервоприводу с номером i
dxl.setGoalPosition(i, buf[i]-20*DEGREE_COEFF); // задание целевого положения -20 градусов от текущего сервоприводу с номером i
}
delay(5000); // задержка в 5 секунд, ожидание, пока сервоприводы займут нужное положение
for (i=1; i<=jointN; i++) {
dxl.setGoalVelocity(i, 50); // задание целевой скорости 50 сервоприводу с номером i
dxl.setGoalPosition(i, buf[i]); // задание начального целевого положения, т.е. +20 градусов от последнего сервоприводу с номером i
}
delay(5000); // задержка в 5 секунд, ожидание, пока сервоприводы займут нужное положение
}
Воспроизведение записанных позиций
Далее рассматривается программа, позволяющая воспроизводить позиции, которые ранее были считаны предыдущей программой. С помощью этой программы можно достичь цели – переноса небольшого предмета из одной точки стола в другую.
Обратите внимание, в этой программе задается двумерный массив, в котором записаны положения сервоприводов. Построчно записаны различные положения манипулятора, а по столбцам – отдельные положения каждого из сервоприводов. Это значит, что массив имеет 5 столбцов и переменное количество строк – число строк зависит от количества воспроизводимых положений (на самом деле число строк и столбцов на 1 больше, так как есть неиспользуемые нулевые строка и столбец; в программе они задаются нулевыми).
В основной части программы с помощью двух циклов перебираются элементы массива. Соответствующие значения массива отправляются на сервоприводы для воспроизведения, в результате чего манипулятор будет отрабатывать положения, заданные в массиве.
Значения положений манипулятора будут предварительно записаны в массив самим пользователем с использованием предыдущей программы.
Таким образом, 2 управляющие программы, работающие в связке, позволят организовать копирующий режим работы манипулятора.
Работает копирующий режим в данном случае следующим образом:
- Считывание и вывод на экран позиций (используется программа для чтения позиций).
Пользователь вручную задает некоторую позицию манипулятора (поворачивая манипулятор рукой и придерживая его). В монитор порта через каждую секунду выводится текущая конфигурация манипулятора. Когда значения построчно начинают повторяться, то манипулятор вручную установлен в целевую позицию. В мониторе порта необходимо выключить автопрокрутку, а затем, выделив курсором одну из нужных строчек (обозначающих целевую позицию), скопировать строку (Ctrl+C). Далее эту строку необходимо будет вставить в программу для воспроизведения позиций.
Повторять до тех пор, пока все позиции для воспроизведения не будут записаны.
- Воспроизведение позиций
Когда все позиции считаны и записаны в массив, необходимо убедиться, что управляющая программа работает корректно. Сделать это можно, проведя компиляцию программы. Также необходимо следить за тем, что переменная, отвечающая за количество строк в массиве, задана верно – под именно текущее количество строк (а оно зависит от пользователя).
Таким образом, предварительно «научив» манипулятор переносить какой-либо предмет (то есть последовательно задавая все положения манипулятора, необходимые для переноса предмета: начальное, наведение на предмет, подход к предмету, схват, перенос, опускание предмета, отпускание предмета), можно добиться того, что манипулятор будет воспроизводить позиции, которым он ранее научился.
#include <Dynamixel2Arduino.h> // подключение библиотеки Dynamixel
// Последовательный порт DXL для платы STEM с OpenCM 9.04. (Если использовать DXL порт
// на самой плате OpenCM 9.04, то нужно использовать Serial1, и из-за кода в драйвере,
// необходимо перед dxl.begin() вызвать Serial1.setDxlMode(true).)
#define DXL_SERIAL Serial3
#define DEBUG_SERIAL Serial // последовательный порт, подключаемый к компьютеру
const uint8_t DXL_DIR_PIN = 22; // номер информационного пина сервоприводов (28 для OpenCM 9.04)
const float DXL_PROTOCOL_VERSION = 2.0; // протокол передачи данных от OpenCM9.04 к сервоприводам
Dynamixel2Arduino dxl(DXL_SERIAL, DXL_DIR_PIN); // инициализация указателя на команды из библиотеки Dynamixel
#define jointN 5 // инициализация константы, обозначающей количество сервоприводов (и ненулевых столбцов массива)
#define pages 7 // задание количества строк массива помимо начального положения
int i = 0; // задание переменной-счетчика i
int j = 0; // задание переменной-счетчика j
int buf[pages+1][jointN+1]={ // инициализация двумерного массива размером (7+1=8 на 5+1=6, 8 на 6), так как в программировании нумерация {0, 5, 4349, -5831, 1062, -6310, }, элементов начинается с 0. Но нулевой элемент строки мы использовать не будем, так как начнем с первого.
{0, 1038, 4349, -5831, 1062, -6310, },
{0, 1038, 4349, -5831, 1062, -6310, },
{0, 1038, 4000, -5622, 1062, -6310, },
{0, 1038, 3608, -5423, 1610, -6310, },
{0, 1038, 3608, -5423, 2440, -6310, },
{0, 1038, 3608, -5423, 2440, -5570, },
{0, 1038, 3608, -5423, 1062, -5570, },
};
void setup() {
DEBUG_SERIAL.begin(57600); // установка скорости обмена данными по последовательному порту компьютера
dxl.begin(1000000); // установка скорости обмена данными по последовательному порту манипулятора
dxl.setPortProtocolVersion(DXL_PROTOCOL_VERSION); // выбор протокола обмена данными
for (i=1; i<=jointN; i++) { // цикл с изменением счетчика цикла i от 1 до 5 с шагом 1 (для перебора всех 5 сервоприводов)
dxl.torqueOff(i); // отключаем крутящий момент, когда изменяем EEPROM область в сервоприводе
dxl.setOperatingMode(i, OP_POSITION); // установка режима работы сервопривода с номером i в качестве шарнира
dxl.torqueOn(i); // включаем крутящий момент сервопривода для удержания целевого положения
}
}
void loop() {
for (i=0; i<=pages; i++) // цикл с изменением i от 0 до 7 с шагом 1
{
for(j=1; j<=jointN; j++) // цикл с изменением j от 1 до 5 с шагом 1
{
dxl.setGoalVelocity(j, 50); // задание целевой скорости 50 сервоприводу с номером j
dxl.setGoalPosition(j, buf[i][j]); // задание целевого положения, получаемого из элемента ([i],[j]) сервоприводу с номером j
}
delay(3000);
}
}
Программирование решения обратной задачи кинематики
В качестве примера ниже приведен программный код решения обратной задачи кинематики, математический расчет которой был выполнен ранее:
#include <Dynamixel2Arduino.h> // подключение библиотеки Dynamixel
// Последовательный порт DXL для платы STEM с OpenCM 9.04. (Если использовать DXL порт на самой плате OpenCM 9.04, то нужно использовать Serial1, и из-за кода в драйвере, необходимо перед dxl.begin() вызвать Serial1.setDxlMode(true).)
#define DXL_SERIAL Serial3
#define DEBUG_SERIAL Serial // последовательный порт, подключаемый к компьютеру
const uint8_t DXL_DIR_PIN = 22; // номер информационного пина сервоприводов (28 для OpenCM 9.04)
const float DXL_PROTOCOL_VERSION = 2.0; // протокол передачи данных от OpenCM9.04 к сервоприводам
Dynamixel2Arduino dxl(DXL_SERIAL, DXL_DIR_PIN); // инициализация указателя на команды из библиотеки Dynamixel
float pi = 3.14159265; // задание константы Пи
float rad2ticks = 2*pi / 16383; // перевод из радиан (0...360 градусов) в условные угловые единицы (0...16383)
float degree_coeff = 16383.0 / 360; // количество условных угловых единиц на 1 градус поворота
#define jointN 5 // задание количества сервоприводов
int pos = 0; // инициализация переменной для начального положения сервопривода
bool input_coords = true; // флаг для печати строки ввода координат в последовательный порт
int start_pos[jointN+1]; // инициализация одномерного массива для положений, размер его задается 5+1=6,
int cur_pos[jointN+1]; // т.к. нулевой элемент мы использовать не будем, так как начнем с первого
int i = 0; // счетчик цикла
int alphas[jointN+1]; // массив целевых угловых позиций 1-5 сервоприводов (в условных единицах)
float X; // желаемое положение схвата по оси X (в метрах) float Y; // желаемое положение схвата по оси Y (в метрах) float Z; // желаемое положение схвата по оси Z (в метрах)
float x; // положение по оси X относительно 2 и 3 звеньев (в метрах) float y; // положение по оси Y относительно 2 и 3 звеньев (в метрах) float z; // положение по оси Z относительно 2 и 3 звеньев (в метрах) float xf; // расчетное положение по оси X относительно 2 и 3 звеньев (в метрах)
float yf; // расчетное положение по оси Y относительно 2 и 3 звеньев (в метрах)
float zf; // расчетное положение по оси Z относительно 2 и 3 звеньев (в метрах)
float d; // вспомогательная расчетная величина расстояние от основания манипулятора до схвата
float betta_d; // вспомогательная расчетная величина угол между осью X и d
float gamma_d; // вспомогательная расчетная величина угол между звеном 2 и d
float alpha1; // целевая угловая позиция 1 сервопривода (в радианах) float alpha2; // целевая угловая позиция 2 сервопривода (в радианах) float alpha3; // целевая угловая позиция 3 сервопривода (в радианах) float alpha4; // целевая угловая позиция 4 сервопривода (в радианах) float alpha5; // целевая угловая позиция 5 сервопривода (в радианах)
float l1 = 0.05; // длина условного 1 звена (в метрах) float l2 = 0.14; // длина условного 2 звена (в метрах) float l3 = 0.14; // длина условного 3 звена (в метрах) float l4 = 0.07; // длина условного 4 звена (в метрах) float l5 = 0.02; // длина условного 5 звена (в метрах)
void setup() {
DEBUG_SERIAL.begin(57600); // установка скорости обмена данными по последовательному порту компьютера
dxl.begin(1000000); // установка скорости обмена данными по последовательному порту манипулятора
dxl.setPortProtocolVersion(DXL_PROTOCOL_VERSION); // выбор протокола обмена данными
for (i=1; i<=jointN; i++) // цикл с изменением i от 1 до 5 с шагом 1
{
dxl.torqueOff(i); // отключаем момент, когда изменяем EEPROM область в сервоприводе
dxl.setOperatingMode(i, OP_POSITION); // установка режима работы сервопривода с номером i в качестве шарнира
}
delay(5000);
for (i=1; i<=jointN; i++)
{
start_pos[i] = int(180 * degree_coeff); // установка начальных положений по центру диапазона 0...360 градусов
dxl.torqueOn(i); // включаем момент сервопривода для удержания целевого положения
}
move_servos(40, start_pos); // перемещаем манипулятор в стартовую позицию
delay(2000);
}
void loop() {
if (input_coords) {
DEBUG_SERIAL.println(«Введите координаты цели x,y,z в сантиметрах через пробел:»);
input_coords = false;
}
if (DEBUG_SERIAL.available()) {
// X = 0.15; //>=l4
// Y = 0.0; //0.0 ///// ЗДЕСЬ НЕОБХОДИМО ЗАДАВАТЬ ТРЕБУЕМОЕ ПОЛОЖЕНИЕ МАНИПУЛЯТОРА
// Z = 0.07; ///// (ИЛИ В SERIAL ПОРТЕ)
parse_input(DEBUG_SERIAL.readString()); // cчитываем из Serial порта координаты X,Y,Z
// Переходим в систему координат относительно 2 и 3 звеньев: alpha1 = atan(Y / X); // угол поворота 1-го звена альфа 1
z = Z l1 + l5; // учитываем длины вертикальных звеньев для целевой координаты Z
x = max(l4, X - l4); // пересчет целевой координаты X
DEBUG_SERIAL.print("x = "); DEBUG_SERIAL.println(x); // строки для отладки
DEBUG_SERIAL.print("y = "); DEBUG_SERIAL.println(y); DEBUG_SERIAL.print("z = "); DEBUG_SERIAL.println(z); DEBUG_SERIAL.print("alpha1 = "); DEBUG_SERIAL.println(alpha1);
d = sqrt(x*x + z*z); // вычисление вспомогательного коэффициента d
// Прежде чем найти нужные углы, найдем побочные betta_d и gamma_d:
gamma_d = acos((l2*l2 + d*d - l3*l3)/(2*l2*d)); betta_d = atan(z / x);
alpha2 = pi/2 - gamma_d - betta_d; // вычисление угла альфа 2
alpha3 = pi - acos((l2*l2 + l3*l3 - d*d)/(2*l2*l3)); // вычисление угла альфа 3
alpha3 = alpha3 - (pi/2 - alpha2); // пересчет в целевое положение 3
сервопривода (учитывается альфа 2)
xf = cos(alpha1)*(l2*sin(alpha2) + l3*sin(pi-alpha3-alpha2)); yf = sin(alpha1)*(l2*sin(alpha2) + l3*sin(pi-alpha3-alpha2)); zf = l2*cos(alpha2) - l3*cos(pi-alpha3-alpha2);
DEBUG_SERIAL.print("xf = "); DEBUG_SERIAL.println(xf); // строки для отладки
DEBUG_SERIAL.print("yf = "); DEBUG_SERIAL.println(yf); DEBUG_SERIAL.print("zf = "); DEBUG_SERIAL.println(zf); delay(1000);
alpha4 = 0; // углы альфа4 и альфа5 принимаются нулевыми
alpha5 = 0; // фактически, если разобраться, альфа - это угол отклонения следующего звена от предыдущего
alphas[1] = start_pos[1] + int(alpha1/rad2ticks); // пересчет в целевое положение 1 сервопривода
alphas[2] = start_pos[2] + int(alpha2/rad2ticks); // пересчет в целевое положение 2 сервопривода
alphas[3] = start_pos[3] + int(alpha3/rad2ticks); // пересчет в целевое положение 3 сервопривода
alphas[4] = start_pos[4] + int(alpha4/rad2ticks); // пересчет в целевое положение 4 сервопривода
alphas[5] = start_pos[5] + int(alpha5/rad2ticks); // пересчет в целевое положение 5 сервопривода
move_servos(40, alphas); // перемещаем манипулятор в целевую позицию
delay(1000); input_coords = true;
}
}
// Разделяет строку по пробелам на числа float. void parse_input(String str) {
int separator = str.indexOf(‘ ‘); // ищем первый пробел в строке:
X = str.substring(0, separator).toInt()/100.0; // переводим строку из сантиметров в метры
int separator2 = str.indexOf(‘ ‘, separator+1); // ищем второй пробел
Y = str.substring(separator+1, separator2).toInt()/100.0;
Z = str.substring(separator2).toInt()/100.0;
}
// Устанавливает целевые положения и пошагово перемещает сервоприводы.
void move_servos(int delay_ms, int* pos) {
int diff = 100*degree_coeff; // разница позиций int dir; // направление движения
int step_degree = 3; // шаг в градусах
while (diff/degree_coeff > step_degree) { // выполняется пока разница позиций больше шага
for (i=1; i<=jointN; i++)
{
cur_pos[i] = dxl.getPresentPosition(i); // получение текущей позиции сервопривода с номером i
if (abs(cur_pos[i] - pos[i]) > 0.7*step_degree*degree_coeff) { // двигаем сервопривод, если он еще далеко от цели
dir = (pos[i]-cur_pos[i])<0 ? -1 : 1; // считаем направление (если <0, то dir=-1)
dxl.setGoalPosition(i, cur_pos[i] + int(step_degree*dir*degree_coeff));
// делаем шаг
}
else
dxl.setGoalPosition(i, pos[i]);
}
delay(delay_ms); // задержка выполнения
diff = 0;
for (i=1; i<=jointN; i++)
diff = max(diff, abs(cur_pos[i] - pos[i])); // нахождение максимальной разницы позиций
}
}