Не так давно на работе довелось столкнуться с таким устройством, как сервомотор. Что это такое и как с ним программно работать я хочу рассказать в этой статье.
После прочтения datasheet'a на это устройство становится понятным, что изначально все сервы рассчитаны на скорость работы в 1 Мб/с. Если сравнивать со скоростью передачей данных энкодерам (9600 Кб/c), то это скорость значительно больше. А это значит, что сразу же подключиться утилитой TESTCOMM мы к ней не сможем (максимальная скорость работы меньше в разы).
Контора, которая выпускает данные сервомоторы, расположена в Китае. Поэтому можно сделать логичное предположение о том, что они имеют свой собственный протокол общения с компьютером, который называется dynamixel.
Перво-наперво необходимо собрать саму схему. Для этого нужно задействовать аккумулятор (желательно 12-вольтовый, иначе 6-вольтовым не получится записать данные в EEPROM. А туда нам нужно сохранять ID сервы и скорость работы с ней). Кстати, сделаю небольшое лирическое отступление...
Уже позже, когда мы тестировали работу нескольких серв в сети, я выставил у второго мотора такой же ID, как и первого. Все были уверены, что они (по подобию нескольких локальных машин с одинаковыми именами) будут конфликтовать. Но не тут-то было! Они одновременно откликались и выполняли одни и те же команды.
Но вернемся к теме.
Собираем преобразователь. Он должен преобразовывать сигнал с одного интерфейса на другой, а именно с USB на RS-485. Подключаем USART к преобразователю, USB - в свободный разъем компьютера. При этом с преобразователя должно идти 4 провода: 2 из них - на питание к аккумулятору, а 2 других идут на линии A и B интерфейса RS-485, далее - на специальную перемычку к сервомоторам.
На этом этапе можно считать, что цепь уже построена. Но перед эксплуатацией рекомендую проверить падение напряжения на аккумуляторе. Если тестер показывает меньше 12V, то лучше ставьте его на зарядку или берите другой.
Теперь, когда у нас все готово к испытаниям, начинаем выставлять параметры на серве. Для этого запускаем программу Dynamixel (скачать ее можно отсюда: http://file.qip.ru/arch/rjxjM5m3/Dynamixel.html ). Кстати, в сети Интернет можно и исходники этой утилиты найти. Нажимаем Full Search. Как только программа найдет нашу серву, тут же нужно нажимать кнопку Stop! В противном случае, поиск, основанный на пинге и прослушивании адресов, продолжится дальше, и программа может вылететь.
Теперь мы имеем окошко, где можем выставить ID нашего устройства и скорость работы с ним. Рекомендуется выделить 16-ый baud rate (скорость 116374 Кб/c). Никакие другие параметры нам больше не нужны. Закрываем программу. Можно убедиться, записались ли данные в энергонезависимую память путем повторного запуска утилиты и поиска в ней сервомотора. Если сохраненные на ней данные соответствуют вашим ожиданиям, значит все прошло успешно!
Теперь поговорим о программировании.
Создаем в MS Visual Studio 2012 новый проект с оконным интерфейсом, язык программирования - C#.
В ранее скачанном архиве с программой содержится библиотека dynamixel.dll , в которой содержатся инструкции по взаимодействию с моторами. Ее необходимо подключить к проекту. Теперь нам понадобится класс, который даст доступ к этим командам.
Созданный класс Dynamixel.cs http://www.everfall.com/paste/id.php?2ajee6tw5vb5
Для инициализации сервомотора нам необходимо последовательно вызвать следующие функции:
1) dynamixel.dxl_initialize() - инициализирует канал для передачи данных. Если функция вернула 1, то инициализация прошла успешно. Если вернула 0, то нужно проверить соединение.
2) dynamixel.dxl_set_baud(16); - напоминаем сервомотору, что с ним мы будем общаться по той скорости, на которую он данный момент и рассчитан.
3) dynamixel.dxl_read_word(ID, P_MODEL_NUMBER_L); - читаем с сервомотора одно слово. Опять же, в случае успешного подключения вернется единица. P_MODEL_NUMBER_L - это целочисленная константа, которая по умолчанию равна 0.
После инициализации сервомотора уже можем посылать данные и устройство должно откликаться! Но надо помнить, что датчики SR518 рассчитаны на поворот на 300 градусов.
Для того чтобы повернуть нашу серву на определенный угол ɸ, выделим старшие и младшие байты для поворота:
int highByte = ɸ / 75; // деление целочисленное
int lowByte = ɸ - highByte * 75;
Осталось заслать в память данные по следующим адресам:
dynamixel.dxl_write_byte(ID, 0x1F, highByte);
dynamixel.dxl_write_byte(ID, 0x1E, (int)(coef * lowByte));
, где 0x1F - адрес старшего байта поворота внутренней функции SetPosition, а 0x1E - адрес младшего байта (см. datasheet, таблица адресов памяти, стр.12).
Если с сервой нужно завершить работу, то у статического класса dynamixel вызываем функцию dxl_terminate().
P.S. Если по статье возникнут какие-либо вопросы, буду рад ответить на них в комментариях.
После прочтения datasheet'a на это устройство становится понятным, что изначально все сервы рассчитаны на скорость работы в 1 Мб/с. Если сравнивать со скоростью передачей данных энкодерам (9600 Кб/c), то это скорость значительно больше. А это значит, что сразу же подключиться утилитой TESTCOMM мы к ней не сможем (максимальная скорость работы меньше в разы).
Контора, которая выпускает данные сервомоторы, расположена в Китае. Поэтому можно сделать логичное предположение о том, что они имеют свой собственный протокол общения с компьютером, который называется dynamixel.
Перво-наперво необходимо собрать саму схему. Для этого нужно задействовать аккумулятор (желательно 12-вольтовый, иначе 6-вольтовым не получится записать данные в EEPROM. А туда нам нужно сохранять ID сервы и скорость работы с ней). Кстати, сделаю небольшое лирическое отступление...
Уже позже, когда мы тестировали работу нескольких серв в сети, я выставил у второго мотора такой же ID, как и первого. Все были уверены, что они (по подобию нескольких локальных машин с одинаковыми именами) будут конфликтовать. Но не тут-то было! Они одновременно откликались и выполняли одни и те же команды.
Но вернемся к теме.
Собираем преобразователь. Он должен преобразовывать сигнал с одного интерфейса на другой, а именно с USB на RS-485. Подключаем USART к преобразователю, USB - в свободный разъем компьютера. При этом с преобразователя должно идти 4 провода: 2 из них - на питание к аккумулятору, а 2 других идут на линии A и B интерфейса RS-485, далее - на специальную перемычку к сервомоторам.
На этом этапе можно считать, что цепь уже построена. Но перед эксплуатацией рекомендую проверить падение напряжения на аккумуляторе. Если тестер показывает меньше 12V, то лучше ставьте его на зарядку или берите другой.
Теперь, когда у нас все готово к испытаниям, начинаем выставлять параметры на серве. Для этого запускаем программу Dynamixel (скачать ее можно отсюда: http://file.qip.ru/arch/rjxjM5m3/Dynamixel.html ). Кстати, в сети Интернет можно и исходники этой утилиты найти. Нажимаем Full Search. Как только программа найдет нашу серву, тут же нужно нажимать кнопку Stop! В противном случае, поиск, основанный на пинге и прослушивании адресов, продолжится дальше, и программа может вылететь.
Теперь мы имеем окошко, где можем выставить ID нашего устройства и скорость работы с ним. Рекомендуется выделить 16-ый baud rate (скорость 116374 Кб/c). Никакие другие параметры нам больше не нужны. Закрываем программу. Можно убедиться, записались ли данные в энергонезависимую память путем повторного запуска утилиты и поиска в ней сервомотора. Если сохраненные на ней данные соответствуют вашим ожиданиям, значит все прошло успешно!
Теперь поговорим о программировании.
Создаем в MS Visual Studio 2012 новый проект с оконным интерфейсом, язык программирования - C#.
В ранее скачанном архиве с программой содержится библиотека dynamixel.dll , в которой содержатся инструкции по взаимодействию с моторами. Ее необходимо подключить к проекту. Теперь нам понадобится класс, который даст доступ к этим командам.
Созданный класс Dynamixel.cs http://www.everfall.com/paste/id.php?2ajee6tw5vb5
Для инициализации сервомотора нам необходимо последовательно вызвать следующие функции:
1) dynamixel.dxl_initialize() - инициализирует канал для передачи данных. Если функция вернула 1, то инициализация прошла успешно. Если вернула 0, то нужно проверить соединение.
2) dynamixel.dxl_set_baud(16); - напоминаем сервомотору, что с ним мы будем общаться по той скорости, на которую он данный момент и рассчитан.
3) dynamixel.dxl_read_word(ID, P_MODEL_NUMBER_L); - читаем с сервомотора одно слово. Опять же, в случае успешного подключения вернется единица. P_MODEL_NUMBER_L - это целочисленная константа, которая по умолчанию равна 0.
После инициализации сервомотора уже можем посылать данные и устройство должно откликаться! Но надо помнить, что датчики SR518 рассчитаны на поворот на 300 градусов.
Для того чтобы повернуть нашу серву на определенный угол ɸ, выделим старшие и младшие байты для поворота:
int highByte = ɸ / 75; // деление целочисленное
int lowByte = ɸ - highByte * 75;
Осталось заслать в память данные по следующим адресам:
dynamixel.dxl_write_byte(ID, 0x1F, highByte);
dynamixel.dxl_write_byte(ID, 0x1E, (int)(coef * lowByte));
, где 0x1F - адрес старшего байта поворота внутренней функции SetPosition, а 0x1E - адрес младшего байта (см. datasheet, таблица адресов памяти, стр.12).
Если с сервой нужно завершить работу, то у статического класса dynamixel вызываем функцию dxl_terminate().
P.S. Если по статье возникнут какие-либо вопросы, буду рад ответить на них в комментариях.
