MotorControl - удаленное управление моторами NXT

Управление моторами робота - практически всегда ключевая задача для программиста. Если устройство не имеет двигающихся частей, то его уже трудно назвать роботом. Так уж получилось, что программирование операций с моторами на LEGO роботах, во многих случаях, достаточно непростое занятие.

На блоге уже публиковались несколько статей (ссылка + ссылка), изучавших этот вопрос. Основные проблемы, с которыми приходится сталкиваться при этом, сводятся к двум пунктам:

• Нельзя повернуть ось мотора в точности на заданный угол поворота
• При одновременном включении двух моторов для перемещения тележки наблюдается рассинхронизация скоростей вращения, что приводит к тому, что робот не едет четко в заданном направлении

Эти проблемы только усугубляются, если робот не автономный, а управляется с компьютера или смартфона. Связано это с задержками передачи команд и задержками опроса енкодеров по USB/Bluetooth и спецификой команды управления моторами, которая используется при такого рода соединении.

Тому, кто интересуется деталями, можно порекомендовать обратиться к LEGO Mindstorms NXT Bluetooth Develope Kit, в котором описаны все API, доступные для использования при удаленном соединении (этот набор документов также будет полезен тем, кто собирается разрабатывать свое приложение под Android устройства или устройства фирмы Apple). Причем не смотря на название одни и те же API используются как для управления по USB, так и при управлении по Bluetooth. Здесь же можно лишь упомянуть, что для операций с моторами используются всего три сообщения: SetOutputState, GetOutputState и ResetMotorPosition.

Практика показывает, что более-менее точное управление моторами все-же возможно: взгляните на Motor PID Block от компании HiTechnic для среды программирования NXT-G и набор функций языка программирования Not eXactly C:

SetMotorRegulationTime(MS_10); SetMotorRegulationOptions(OUT_REGOPTION_NO_SATURATION);   PosRegSetMax(OUT_A, 75, 15); PosRegEnable(OUT_A); PosRegSetAngle(OUT_A, 360); Wait(5000); PosRegAddAngle(OUT_A, 360); Wait(5000);

Но эти способы работают только при выполнении программ непосредственно на NXT блоке. Следовательно, если написать программу для выполнения на блоке и передавать ей команды посредством Bluetooth/USB, то можно достигнуть достаточно аккуратного взаимодействия с моторами.

Именно по этому пути и пошел проект "The RWTH - Mindstorms NXT toolbox for MATLAB" - в рамках этого проекта была разработана программа MotorControl для NXT блока, взаимодействующая с компьютером по Bluetooth или USB для обеспечения точного движения моторов.

Вот полный набор функциональности предоставляемой данной программой:

• достаточно точное перемещение на любое количество градусов (+/- 1 градус в 90% случаев)
• постепенная остановка
• защита от остановки мотора в процессе снижения скорости (постепенной остановки)
• постепенный старт
• синхронизированное движение (для двухмоторных тележек)
• независимый контроль всех трех моторов
• отмена команд
• независимый опрос енкодеров
• определение окончания движения

Общий алгоритм работы с программой MotorControl заключается в том, что сначала необходимо запустить ее, а потом, во время ее работы, передавать с компьютера или смартфона команды в виде строк-сообщений. При этом распознаются следующие сообщения:

• CONTROLLED_MOTORCMD - основная команда, ее параметры говорят каким моторам двигаться, как именно и как долго.
• RESET_ERROR_CORRECTION - команда для сброса встроенного в FW механизма коррекции ошибок и для сброса состояний енкодеров.
• ISMOTORREADY - команда нужна, чтобы определить состояние одного мотора: выполняется ли какая-то команда для него или нет. Команда в отличие от других - двунаправленная, т.е. после посылки сообщения программа на компьютере/смартфоне должна опрашивать блок для получения ответа.
• CLASSIC_MOTORCMD - команда по сути является оберткой над SetOutputState и используется для того, чтобы не использовать "продвинутый" алгоритм движения, а заиспользовать стандартный, предусмотренный firmware, или в случае когда необходимо выполнить плавную остановку или сменить мощность на моторе. За более развернутой информацией нужно обратиться к официальной документации.

Как именно эти команды кодируются в виде строк, а также насколько важно соблюдать задержки между посылкой команд и какие по длительности задержки должны можно прочитать на странице проекта.

Следует также отметить, что среди этих команд нет таких, которые бы опрашивали состояние енкодеров моторов. Дело в том, что для этого могут быть использованы обычные средства, предоставляемые программисту (GetOutputState), которые могут быть использованы в любое время вне зависимости от того какое сообщение было послано последним и выполняет мотор или нет какую-то команду.

Разработчики не скрывают исходный код модуля MotorControl и даже выложили его для свободного использования. Как результат, после создания этот инструмент начал использоваться не только в рамках проекта RWTH, но и в других библиотеках обеспечивающих взаимодействие с роботами в таких языках, как Python (nxt-python) и C# (MindSqualls). На самом деле, работу с ним несложно самостоятельно реализовать также и на других языках программирования, например, LabView и Java. Последняя, в свою очередь, позволит достаточно точно управлять моторами с Android устройств.

Еще одна польза, которая может быть извлечена из этого проекта, - поскольку исходный код для него открыт, а языком программирования является NXC, то можно ознакомиться со способами, которые используются для точного позиционирования двигателей, а потом применять подобные алгоритмы в своих программах для создания уже автономных роботов.