ГЛАВА 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. Проверка на пустые строки таблицы и ошибки реализована.
Рис. 5.1 Вид функции 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.
Рис. 5.2 Вызов функции TrackingCamParseBlobs
Функция для опроса модуля TrackingCam и вывода результатов на экран готова. Теперь необходимо вернуться в тело основной программы и вписать туда вызов функции TrackingCamParseBlobs с помощью инструкции CALL. Еще необходимо понимать, что опрос камеры не происходит мгновенно, так что между вызовами функции TrackingCamParseBlobs необходимо добавить паузу в 0.033 секунды с помощью таймера высокого разрешения (рис. 5.2).
Программа готова. Если ее загрузить в контроллер робота, открыть окно Debagging и запустить процесс отладки и саму программу на контроллере, то в окне (рис. 5.3) появятся строки, содержащие информацию о распознанных областях.
Рис. 5.3 Пример размещения двух однотонных объектов перед камерой
Если в зоне видимости камеры будут находится 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.
Рис. 5.4 Вывод данных о распознанных объектах в окне Debagging
Рис. 5.5 Вид функции TrackingCamParseBlobs
в случае получения данных о составных объектах
Поскольку параметров, доступных для чтения при опросе данных об объектах меньше чем в случае опроса данных об областях, то функция TrackingCamParseBlobs в этом случае будет отличаться только тем, что на экран будет выводить только пять параметров (рис. 5.5). Во всем остальном функция будет повторять функцию TrackingCamParseBlobs.
Center-nav