Python и MotorControl

(C)

В предыдущей заметке уже упоминалось о том, что модуль nxt-python для языка программирования Python уже имеет встроенную поддержку инструмента по управлению моторамиMotorControl.

По сути, это заключается в следующем:

• в составе архива модуля есть скомпилированный исполняемый модуль MotorControl22.rxe (22 - это версия модуля - 2.2), который можно загрузить на NXT блок с помощью поставляющегося в архиве скрипта nxt_push или программы NeXTTool, доступной для загрузке на сайте BricxCC. Тут же есть и исходный код, который можно собрать исполняемый файл с помощью компилятора NXC непосредственно или с помощью среды BricxCC
• в самом модуле nxt-python доступен набор методов класса MotCont, реализованного в под-модуле nxt.motcont, обеспечивающих обмен сообщениями по USB/Bluetooth с управляющей программойMotorControl. Примечательным фактом является, что никакого специального объекта этого класса создавать не надо. Он автоматически создается в результате инициализации объектаBrick, т.е., например, доступен сразу после того, как объект Brickвернется функцией find_one_brick(). К объекту класса MotCont в этом случае можно обращаться через имя mc (см. пример ниже).

Работа с MotorControl происходит по следующей схеме.

1. Прежде, чем программа начнет передавать на NXT блок сообщения по Bluetooth/USB, исполняемый модуль MotorControl должен быть запущен на блоке. Это может быть сделано руками с помощью NXT кнопок, либо автоматически из программы с помощью метода start().
2. По ходу работы программы можно передавать сообщения на блок. Сообщения передаются с помощью следующих методов:
   
   • cmd(порт, мощность, градусы, контроль скорости, плавный старт, торможение) - посылка сообщенияCONTROLLED_MOTORCMD, где
     
     • порт - задается через стандартные переменные модуля nxt.motor - PORT_A, PORT_B, PORT_C или через числа: 3 - AB, 4 - AC, 5 - BC, 6 - ABC
     • мощность задается также как и обычно: 0..100 - вращается вперед, -100..0 - вращается назад
     • градусы - от положительные числа 0 до 999999 - на сколько повернуть ось двигателя. Следует отметить, что поскольку функции работы с модулем MotorControl по своей природе просто передают сообщение на блок, то они не ждут окончания выполнения команды - даже если мотор поворачивается на 5000 градусов, управление вернется в программу сразу после запуска двигателя, т.е. параллельно с движением моторам в программе можно выполнять какие-то другие вычисления.
     • контроль скорости - 0 или 1 - нужно ли обеспечивать контроль скорости при большой нагрузке на моторы.
     • плавный старт - 0 или 1 - плавный набор скорости
     • торможение - 0 или 1 - после конца движения выполнить торможение, а не просто отключить подачу энергии на моторы. Причем после торможения, мотор будет стремиться сохранить заданное положение оси - энергия все еще будет подаваться на мотор, до явной отмены.
   • move_to(порт, мощность, градусы, контроль скорости, плавный старт, торможение) - почти тоже самое, что и cmd()только ось мотора движется относительно какой-то базовой точки. Иными словами, - до какого положения оси мотора выполнить поворот. За базовую точку используется положение оси либо после включения блока, либо после команды сброса датчика поворота (енкодера). Градусы, в отличие от cmd(), могут быть отрицательными.
     Например, первый вызов move_to c числом градусов 90 повернет ось на 90 градусов, последующий вызов move_to со значением 45 не повернет ось вперед, а наоборот сдвинет назад, а если после этого передать в move_to значение -180, то ось будет снова вращаться назад, но уже явно больше, чем на пол-оборота.
     
   • reset_tacho(порт) - посылка сообщенияRESET_ERROR_CORRECTION - произведет сброс датчика поворота двигателя в заданном порту.
   • is_ready(порт) - ответствен за сообщение ISMOTORREADY. Делает опрос - выполняет ли MotorContol сейчас какую-то команду для мотора в данном порту или нет. Иными словами, выполняется ли сейчас вращение мотора или нет.
   • set_output_state(порт, мощность, градусы, контроль скорости) - как результат вызова данного API посылается сообщение CLASSIC_MOTORCMD. Используется, когда неизвестно на какое количество градусов нужно повернуть ось - просто передается 0 в параметре градусы. Также данный метод используется, когда нужно отменить предыдущую команду (остановить мотор), посланную черезset_output_state().
3. Как было сказано выше, после вызова метода посылки сообщения на тот или иной мотор, управление передается в программу, поэтому здесь можно выполнять вычисления или продолжать работать с NXT блоком: опрашивать датчики и енкодеры, тем самым меняя поведение робота
4. В конце программы можно автоматически остановить модульMotorControl, выполнив метод stop().

Примеры ниже помогут лучше понять вызов каждого конкретного метода.

Пример #1. Запуск и остановка модуля MotorControl. Поворот мотора и опрос состояния.

from nxt.locator import find_one_brick
from nxt.motor import *
from time import sleep, clock
 
brick = find_one_brick()
mc = brick.mc
 
# Запустить MotorControl модуль на NXT блоке
mc.start()
 
# Передать сообщение: повернуть мотор А на 1000 градусов
mc.cmd(PORT_A, 50, 1000)
 
# Подождать пока мотор не завершит движение
# Таймер выводится просто так, чтобы показать, что цикл работает
timestart = clock()
while not mc.is_ready(PORT_A):
    print clock()-timestart
 
# Остановить модуль MotorControl
mc.stop()
 
sleep(2)

Пример #2. Поворот двигателя и возврат на предыдущую позицию. Установка новой "базовой" позиции.

from nxt.locator import find_one_brick
from nxt.motor import *
from time import sleep, clock
from random import randint
 
def WaitForCompletion(m, port):
    while not m.is_ready(PORT_A):
        pass
 
brick = find_one_brick()
mc = brick.mc
 
# Запустить MotorControl модуль на NXT блоке
mc.start()
 
# Передать сообщение: повернуть мотор А в позицию 360 градусов
mc.move_to(PORT_A, 50, 360)
# Подождать окончания
WaitForCompletion(mc, PORT_A)
 
# Передать сообщение: вернуть мотор А в "базовую позицию"
mc.move_to(PORT_A, 50, 0)
# Подождать окончания
WaitForCompletion(mc, PORT_A)
 
# Повернуть мотор А на случайное количесво градусов
mc.cmd(PORT_A, 100, randint(90, 180))
# Еще один способ подождать окончания - не учитывает скорости
# вращения мотора
sleep(2)
 
# Сбросить енкодер мотора А в ноль, т.е. текущее положение
# становится "базовой позицией"
mc.reset_tacho(PORT_A)
 
# Повернуть мотор А в позицию -180 градусов относительно новой
# базовой позиции, а затем повернуть на 90 градусов
mc.move_to(PORT_A, 100, -180)
sleep(2)
mc.move_to(PORT_A, 100, 90)
sleep(2)
 
# Остановить модуль MotorControl
mc.stop()
 
sleep(2)

Пример #3. Запустить моторы для поворота на 5000 градусов, но остановить движение, если датчик касания столкнулся с препятствием.

from nxt.locator import find_one_brick
from nxt.motor import *
from nxt.sensor import *
from time import sleep
 
brick = find_one_brick()
mc = brick.mc
 
# Создать объекты для каждого мотора
# Будут использоваться для торможения
engine1 = Motor(brick, PORT_A)
engine2 = Motor(brick, PORT_B)
 
# Запустить MotorControl модуль на NXT блоке
mc.start()
 
# Создать объект (после запуска MotorControl) для датчика касания
bumper = Touch(brick, PORT_1)
 
# Передать сообщение: повернуть моторы А и B на 5000 градусов
mc.cmd(3, 50, 5000)
 
# Ждать, пока либо мотор закончит движение 
while not mc.is_ready(PORT_A):
    # либо датчик касания обнаружит препятствие
    if bumper.is_pressed():
        # остановить моторы
        engine1.brake()
        engine2.brake()
        break
 
sleep(0.5)
 
# Остановить модуль MotorControl
mc.stop()
 
# Перестать подавать энергию на моторы после вызова brake() 
engine1.idle()
engine2.idle()

Пример #4. Посылка сообщений через set_output_state().

from nxt.locator import find_one_brick
from nxt.sensor import *
from time import sleep
 
brick = find_one_brick()
mc = brick.mc
 
# Запустить MotorControl модуль на NXT блоке
mc.start()
 
# Создать объект (после запуска MotorControl) для датчика касания
bumper = Touch(brick, PORT_1)
 
# Передать сообщение: запустить моторы А и B - ехать вперед
mc.set_output_state(3, 50, 0)
 
# Ждать, пока датчик касания обнаружит препятствие
while not bumper.is_pressed():
    pass
 
# остановить моторы
mc.set_output_state(3, 0, 0)
 
sleep(0.5)
 
# Остановить модуль MotorControl
mc.stop()