Глава 5. Работа модуля TrackingCam контроллером СМ-530
Получение данных о распознанных областях
Модуль TrackingCam является Dynamixel-совместимым устройством, что позволяет подключать его к контроллерам корейского производителя робототехнических комплектующих ROBOTIS без каких-либо дополнительных переходников. Для этого необходимо используя стандартный кабель для подключения сервоприводов Dynamixel соединить модуль TrackingCam и контроллер CM-530. При этом не имеет значения к какому именно разъему на модуле и к какому именно порту на контроллере будет выполнено подключение, поскольку идентификация Dynamixel-совместимого устройства в системе выполняется по его ID — номеру. По умолчанию модуль TrackingCam имеет ID равный 51. При необходимости этот ID можно изменить при настройке модуля.
При использовании протокола Dynamixel телекамера имеет регистровую модель управления — доступно чтение из памяти телекамеры объемом 256 байт, в которой по адресу 16 (0x10) располагается таблица с данными. Содержание таблицы зависит от режима работы, выбранных алгоритмов обработки изображений и настроек сортировки выходных данных. Если активно только распознавание цветных областей, то данные записываются как показано в таблице 5.1.
Порядок байт соответствует протоколу Dynamixel — младший байт первый. Информация о распознанных областях размещается подряд в первых ячейках таблицы в порядке обнаружения, пустые ячейки заполняются значением 0xFF (255).
Разберемся как получать данные о распознанных одноцветных областях. Для этого настроим телекамеру на распознавание одной области, например оранжевого мяча, как в рассмотренном ранее примере. После этого отключим модуль от компьютера и подключим его к контроллеру CM-530. Для написания программы получения данных в данном случае воспользуемся средой разработки RoboPlus Task 2.0.
Создадим новый проект, добавим в него инструкции, обозначающие начало и конец программы и вне основного тела программы создадим функцию TrackingCamParseBlobs — она и будет отвечать за опрос модуля и вывод данных на экран. Следующим шагом внутри функции необходимо указать ID модуля, и начальные адреса памяти с которых начнется его опрос. С помощью инструкции LOAD укажем переменной TrackingCamID
| Адрес | Смещение | |||||||||||||||
|---|---|---|---|---|---|---|---|---|---|---|---|---|---|---|---|---|
| 0 | 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | 12 | 13 | 14 | 15 | |
| 0x10 | Тип | - | Cx | Cy | S/4 | L | R | T | B | |||||||
| 0x20 | Тип | - | Cx | Cy | S/4 | L | R | T | B | |||||||
| ... | ||||||||||||||||
| 0xE0 | 0xFF | - | - | - | - | - | - | - | - | |||||||
| 0xF0 | 0xFF | - | - | - | - | - | - | - | - |
| Тип – номер шаблона, по которому распознана область. Cx, Cy – положение центра масс области.
Тип – номер шаблона, по которому распознана область. Cx, Cy – положение центра масс области. S – площадь (делится на 4 для экономии памяти). L – положение левой границы области по оси X. R – положение правой границы области по оси X. T – положение верхней границы области по оси Y. B – положение нижней границы области по оси Y.
Таблица 5.1 Размещение данных о цветных областях в памяти модуля при использовании протокола Dynamixel
константное значение 51. Аналогичным образом укажем адрес, с которого начнется опрос модуля — TrackingCamBlobStorageAddr — его значение 16 — это адрес 1-й строки таблицы данных (первые 16 байт представляют собой системную информацию и для нас интереса не представляют). С помощью инструкции LOAD создадим переменную addr, которой присвоим значение переменной TrackingCamBlobStorageAddr. Переменная addr будет использоваться для записи в нее текущего адреса, чтение которого выполняется, ее значение будет увеличиваться по мере продвижения при чтении таблицы.
Чтобы получить данные о всех найденных областях необходимо перебрать всю результирующую таблицу до первой пустой записи (0xFF). Для этого используем цикл LOOP FOR, который будет выполняться столько раз, сколько будет указано. Поскольку у нашей таблицы 16 строк, то циклу LOOP FOR указываем диапазон итераций — от 0 до 15. Теперь, непосредственно для получения всех данных необходимо перебрать все элементы таблицы построчно сверху вниз. Для этого начнем с того, что с помощью инструкции LOAD прочитаем в переменную BlobType значение, располагающееся по адресу addr Dynamixel-устройства с ID, записанным в переменной TrackingCamID. Такое обращение осуществляется путем выбора пункта Dynamixel Device — Custom и указания в поле ID переменной TrackingCamID и в поле Address переменную addr. После чего с помощью инструкции COMPUTE смещаем адрес на 2 байта, т.к. поле которое читаем состоит из 16 бит. Здесь необходимо понимать, что в переменную BlobType запишется тип области. Однако, если поле таблицы пустое или произошла какая-либо ошибка, туда могут быть записаны значения 255 (означает пустую запись в таблице) и -1 (ошибка). В случае, если такое случится, то необходимо прервать выполнение цикла LOOP FOR. Для контроля таких ситуаций добавим условный переход IF c 3-мя условиями: если значение переменной BlobType равно 255, -1 или 248 (контроль отсутствия этого значения необходим из-за особенностей протокола Dynamixel). В случае срабатывания одного из этих трех условий выполнение цикла должно прерваться и для этого в условный переход IF добавим инструкцию BREAK LOOP. Проверка на пустые строки таблицы и ошибки реализована.
![]()
Вид функции TrackingCamParseBlobs
Продолжим опрашивать модуль. Вне тела условного перехода IF, но внутри цикла LOOP FOR будем создавать конструкции из инструкций LOAD, задача которой считать значение по указанному адресу и присвоить его некой переменной, и COMPUTE, задача которой выполнить смещение адреса. Таким образом выполним присвоение значений следующим переменным: BlobCx – центр области по оси х в пикселях, BlobCy — центр области по оси y в пикселях, BlobArea — площадь области в пикселах, которую еще необходимо домножить на 4 (т.к. при передаче данных значение площади делится на 4, для того чтобы результат поместился в 16 бит), BlobLeft — значение левой границы области в пикселях, BlobRight — значение правой границы области в пикселях, BlobTop — значение верхней границы области в пикселях и BlobBottom — значение нижней границы области в пикселях. Таким образом будут получены значения основных характеристик распознанных областей. Для вывода полученных значений на экран воспользуемся рассмотренными ранее командами Print Screen — для вывода данных без перехода на новую строку и Print Screen with Line — для перехода на новую строку после отображения данных. В результате должна получится конструкция, изображенная на рисунке 5.1.
![]()
Вызов функции TrackingCamParseBlobs
Функция для опроса модуля TrackingCam и вывода результатов на экран готова. Теперь необходимо вернуться в тело основной программы и вписать туда вызов функции TrackingCamParseBlobs с помощью инструкции CALL. Еще необходимо понимать, что опрос камеры не происходит мгновенно, так что между вызовами функции TrackingCamParseBlobs необходимо добавить паузу в 0.033 секунды с помощью таймера высокого разрешения .
Программа готова. Если ее загрузить в контроллер робота, открыть окно Debugging и запустить процесс отладки и саму программу на контроллере, то в окне появятся строки, содержащие информацию о распознанных областях.
![]()
Пример размещения двух однотонных объектов перед камерой
Если в зоне видимости камеры будут находится 2 одинаковых объекта, то данные будут выдаваться последовательно о двух фиксируемых областях. Здесь:
0 - BlobType 141 - BlobCx 125 - BlobCy 380 - BlobArea 126 - BlobLeft 158 - BlobRight 120 - BlobTop 133 - BlobBottom 1111111 - разделитель областей 3333 3333 - разделитель запросов
Таким образом в этом примере на экран выводятся данные о двух распознанных областях, относящихся к одному цветовому шаблону. Это видно по параметру BlobType, который у каждой строки одинаков и по различию других параметров — положения области и ее размеров. Таким образом можно получать данные с камеры о распознанных областях и классифицировать их.
Получение данных о распознанных объектах
Процесс получения данных о распознанных объектах полностью аналогичен процессу получения данных о распознанных одиночных областях за одним исключением — таблица данных заполнена иначе. Если при использовании протокола Dynamixel включено распознавание составных объектов, то данные записываются как показано в таблице 5.2.
| Адрес | Смещение | |||||||||
|---|---|---|---|---|---|---|---|---|---|---|
| 0 | 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | |
| 0x10 | Тип | - | Cx | Cy | S/4 | L | ||||
| 0x20 | Тип | - | Cx | Cy | S/4 | L | ||||
| ... | ||||||||||
| 0xE0 | 0xFF | - | - | - | - | - | ||||
| 0xF0 | 0xFF | - | - | - | - | - |
| Тип – номер шаблона, по которому распознан объект.
Тип – номер шаблона, по которому распознан объект.
Cx, Cy – положение начала координат объекта, по умолчанию центр базового узла. Угол – угол поворота объекта.
Размер – размер объекта в заданной мере (делится на 4 для экономии памяти)
Таблица 5.2 Размещение данных о составных объектах в памяти модуля при использовании протокола Dynamixel
Доступных для чтения данных меньше, потому что сортировка и отбор интересующих объектов настраиваются отдельно и производятся внутри телекамеры. Обозначим переменные для считывания параметров при работе с объектами следующим образом:
ObjCx — расположение центра объекта по оси х; ObjCy — расположение центра объекта по оси y; ObjAngle — угол, на который повернут объект; ObjSize — размер объекта в выбранной мере
Вывод этих параметров на экран, по аналогии с предыдущим примером, выглядит как на рисунке 5.4.
![]()
Вывод данных о распознанных объектах в окне Debugging
![]()
Вид функции TrackingCamParseBlobs
в случае получения данных о составных объектах
Поскольку параметров, доступных для чтения при опросе данных об объектах меньше чем в случае опроса данных об областях, то функция TrackingCamParseBlobs в этом случае будет отличаться только тем, что на экран будет выводить только пять параметров . Во всем остальном функция будет повторять функцию TrackingCamParseBlobs.