RADU: управление движениями робота с помощью контроллера геймпада с помощью Python

Мой робот RADU — это двухколесный робот, который сочетает в себе пользовательское оборудование для датчиков и двигателей, Raspberry Pico и моторный щит L293D, а также покупной роботизированный манипулятор, который управляется с помощью щита Raspberry Pi. Для обеих частей робота я хочу использовать один и тот же механизм и устройство управления: игровой контроллер. Чтобы достичь этой цели, в прошлой статье был дан обзор различных библиотек. Эта статья — учебник по написанию программного обеспечения для контроллера. Вы узнаете, как использовать библиотеку Python Piborg Gamepad, и как использовать нажатые кнопки или джойстик для управления движениями колес робота и его руки.

Эта статья первоначально появилась в моем блоге admantium.com.

Библиотека геймпада

В качестве библиотеки для геймпада я выбрал Piborg Gamepad. Эта универсальная библиотека Python предоставляет простую абстракцию для подключения пользовательских геймпадов, ориентированных на игровые консоли. Она может определять состояние всех кнопок и осей, например, на планшете PlayStation, включая d-pad и аналоговый стик. И он предоставляет различные модели программирования для доступа к состоянию геймпада, например, с помощью слушателей событий, которые вызывают обратные вызовы, или непрерывного цикла, который получает состояние всех кнопок.

После короткого периода времени, когда простое нажатие кнопки приводило робота в движение, я решил использовать эту библиотеку для полного программирования моего робота. Давайте начнем с простого наброска.

Конфигурация кнопок

Первая часть — это импорт библиотеки и настройка переменных для кнопок выбранного геймпада. Для этого просмотрите конфигурацию геймпада и возьмите из этого файла определенные названия кнопок для вашего конкретного геймпада. Затем переведите их в осмысленные имена. Вот моя схема для доступа к джойстику, цифровому пэду и некоторым кнопкам:

import lib.Gamepad as Gamepad

# Gamepad settings
gamepadType = Gamepad.PS4

# Axis
joystickSpeed = 'LEFT-Y'
joystickSteering = 'RIGHT-X'

digital_x = 'DPAD-X' 
digital_y = 'DPAD-Y' 

# Button
button_ps = 'PS'
button_triangle = "TRIANGLE"

Подключение контроллера

После настройки кнопок мы напишем метод, который будет ждать подключения геймпада. Как только это условие станет истинным, мы создадим объект геймпада, который будет инстанцирован с конфигурацией. Этот код выглядит следующим образом:

def wait_for_gamepad():

  # Wait for a connection
  if not Gamepad.available():
    log_to_stdout_and_mqtt('Please connect your gamepad...')
    while not Gamepad.available():
      time.sleep(1.0)

  gamepad = gamepadType()
  return gamepad

Обнаружение нажатий кнопок геймпада

Следующий шаг — запуск главного цикла программы, который будет постоянно проверять состояние кнопок. Я решил использовать простой цикл, который постоянно опрашивает кнопки и выполняет некоторые команды. Давайте начнем с простого и определим слушателя для кнопок.

def main_loop(gamepad)
  pollInterval = 0.01
  gamepad.startBackgroundUpdates()
  try:
    while gamepad.isConnected():
      if gamepad.beenPressed(button_triangle):
        log_to_stdout("Triangle Button is Pressed")

        # Sleep for our polling interval
        time.sleep(pollInterval)
  finally:
    # Ensure the background thread is always terminated when we are done
    gamepad.disconnect()

Этот метод выполняет несколько действий:

• pollInterval определяет, как часто в секунду будут проверяться состояния кнопок.
• Вся логика обнаружения кнопок заключена в блок try...catch, в основном для того, чтобы освободить геймпад, если что-то пойдет не так — в качестве альтернативы вы можете определить собственный контекстный менеджер для обработки фаз установки и отключения.
• Состояние кнопок геймпада проверяется с помощью метода beenPressed(), который получает аргумент ранее определенной раскладки кнопок. Как показано здесь, вы можете использовать операторы if для проверки их состояния, а затем выполнить какую-либо команду.
• В конце цикла выдается команда sleep с определенным pollInterval.

Хорошо, теперь давайте посмотрим, как проверить наличие осей d-pad и рулевого управления.

Определение цифровой и аналоговой оси

Во время настройки кнопок мы уже определили пользовательские имена для аналоговых джойстиков и d-pad. Затем внутри главного цикла мы можем определить их состояние, как показано в следующем фрагменте кода.

def main_loop(gamepad)
  #...
  try:
    while gamepad.isConnected():
      # ...
      speed = - gamepad.axis(joystickSpeed)
      steering = - gamepad.axis(joystickSteering)

      command = gamepad.axis(digital_x)

      log_to_stdout(f'Speed {speed}')
      log_to_stdout(f'Steering {steering}')

      engine_move(speed, steering, command)   

      time.sleep(pollInterval)
  finally:
    # Ensure the background thread is always terminated when we are done
    gamepad.disconnect()

Для доступа к значениям геймпада используется метод axis() и передается имя кнопки. Значения представляют собой плавающие значения разной гранулярности: Для кнопок d-pad это либо 0.0, либо 1.0, а для аналоговых стиков это положительное или отрицательное 16-битное число с максимальным абсолютным значением 32.756. Обращайтесь с этими значениями так, как это требуется вашему роботу.

Тонкая настройка элементов управления

Этот шаг в значительной степени зависит от поведения вашего конкретного робота. Мой робот постоянно слушает команды, обрабатывает их и завершает выполнение команды после 300 мс бездействия отправителя. Это означает, что отправитель должен постоянно передавать положение джойстика, чтобы робот двигался только тогда, когда нажимается любая из кнопок.

Это требование обеспечивается следующим участком кода:

def main_loop(gamepad)
  #...
  try:
    while gamepad.isConnected():
      if (mode == MOVE_MODE):
        speed = -gamepad.axis(joystickSpeed)
        steering = -gamepad.axis(joystickSteering)

        if((speed != 0.0 or steering != 0.0) and not isStopped):
          last_speed = speed
          last_steeering = steering
          engine_move(speed, steering)   
        else:
          isStopped = True

      time.sleep(pollInterval)
  finally:
    # Ensure the background thread is always terminated when we are done
    gamepad.disconnect()

Управление движениями руки

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

Первая функция, которая нам необходима, — это различать различные режимы работы контроллера. Это состояние хранится в следующих переменных:

MOVE_MODE = 'MOVE_MODE'
ARM_MODE = 'ARM_MODE'

Внутри основного цикла нам нужно сделать следующее расширение для изменения текущего режима управления:

def main_loop(gamepad)
  #...
  while gamepad.isConnected():
    if gamepad.beenPressed(button_ps):
      if (mode == MOVE_MODE):
        mode = ARM_MODE
      else:
        mode = MOVE_MODE

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

def main_loop(gamepad)
  #...
  try:
    while gamepad.isConnected():
      #...
      if (mode == MOVE_MODE):
        # ...

      if (mode == ARM_MODE):
        gripper_x = gamepad.axis(digital_x)
        gripper_y = -gamepad.axis(digital_y)

        if (gripper_x != 0.0):
          arm.move_incr(SERVO_G, 4*int(gripper_x))
          log_to_stdout_and_mqtt("X {}, Y {}".format(gripper_x, gripper_y))
        if (gripper_y != -0.0):
          arm.move_incr(SERVO_Z, 4*int(gripper_y ))
          log_to_stdout_and_mqtt("X {}, Y {}".format(gripper_x, gripper_y ))

        arm_x = gamepad.axis(joystickSteering)
        arm_y = -gamepad.axis(joystickSpeed)

        if (arm_x != 0.0):
          arm.move_incr(SERVO_X, int(100*arm_x)/4)
          log_to_stdout_and_mqtt("SERVO_X {} => {}".format(arm_x, int(100*arm_x)/4))
        if (arm_y != -0.0):
          arm.move_incr(SERVO_Y, int(100*arm_y)/4)
          log_to_stdout_and_mqtt("SERVO_Y {} => {}".format(arm_y,int(100*arm_y)/4))

        if gamepad.beenPressed(button_triangle):
          arm.center()

      time.sleep(pollInterval)
  finally:
    # Ensure the background thread is always terminated when we are done
    gamepad.disconnect()

Заключение

В этой статье было показано, как использовать библиотеку Python Piborg Gamepad. Вы узнали, как определить расположение кнопок, как запустить программу с ожиданием подключенного геймпада, а также как постоянно проверять состояние кнопок, цифрового пэда и джойстиков. Все это вместе представляет собой небольшой и легко читаемый скрипт, который вы можете адаптировать к требованиям вашего робота. В статье было конкретно показано, как использовать эту библиотеку для управления движениями колес и рук моего робота RADU. В следующей и последней статье я подведу итоги своего путешествия по созданию собственного робота.