Управляемый робот-футболист

Давненько не было вестей от нашей команды. Вы думаете что мы забросили роботов? Не дождетесь!) С середины января находимся в режиме планомерной подготовки к ВРО, снова готовимся к футболу роботов. В этом году алгоритмы машин снова переписаны заново и во многом не повторяют логику работы наших футболистов предыдущих поколений.

Учебный процесс тоже не стоял на месте, в нашей копилке теперь вот такой крупнокалиберный арсенал: "Введение в программирование (C++)" от Академии Яндекса, "Программирование на Python", "Python: основы и применение", "Введение в Linux" от Института биоинформатики, "Введение в архитектуру ЭВМ. Элементы операционных систем" от Computer Science Center.

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

Поиграть против своей автономной команды очень интересно и полезно:

• Зная слабые места своих автономных роботов можно создавать на поле такие игровые ситуации, в которых они проявятся. Это важно для отладки алгоритмов и конструкций.
• Управляя роботом ты моделируешь в голове работу некого алгоритма, который затем может быть перенесен в программу автономного игрока
• Это реальный драйв - ты играешь в игру не на экране, а в реальном мире, с полноценной физикой и красивыми текстурами. Обзор 360 градусов и высокое разрешение!

Так как мы находимся в процессе изучения языка Python, то интересной учебной задачей стала реализация такого управляемого робота-оппонента на базе ev3dev. Можно конечно "не заморачиваться" и использовать смартфон и написанное кем-то приложение, но это не наш путь. На борту у EV3 есть Bluetooth, у китайцев на Aliexpress - дешевые блютузные джойстики - почему бы не поуправлять роботом с реальных кнопок и стика? Используя стандартное ПО LEGO EV3 такую связку заставить работать невозможно, ev3dev открывает перед нами такую возможность.

Для начала сам робот. Традиционно подготовили инструкцию по сборке в Lego Digital Designer, скачать можно по ссылке. В конструкции умышленно не использовали механизм удара по мячу, чтобы у начинающих не было соблазна собрать на базе этой инструкции автономного робота.

Чтобы джойстик заработал с EV3, необходимо "спарить" устройства привычным образом, после чего в /dev/input должен появиться новый девайс:

robot@ev3dev:~$ ls /dev/input
by-path  event0  event1  event2

Кроме Bluetooth-джойстика можно использовать беспроводной USB-джойстик, воткнув его USB-приемник в соответствующий порт на роботе. Беспроводная клавиатура с интерфейсом USB или Bluetooth тоже подойдет. Технически роботом можно управлять используя даже беспроводную мышку, но это вероятно не особенно удобно. Главное условие - после подключения устройства оно должно появляться в устройствах ввода, в /dev/input

Для получения данных с HID-устройств мы использовали стандартный модуль Python evdev. Ничего доустанавливать в ev3dev не требуется.

Нашу программу для робота можно скачать по ссылке. Основная фишка управления по сравнению со "смартфонным" - реализация плавного разгона и торможения, что обеспечивает комфортное управление. Выглядит программа следующим образом:

#!/usr/bin/env python3

# Подключаем модуль для управления EV3
from ev3dev.ev3 import *
# Подключаем модуль для чтения данных с HID-устройств
import evdev

# Создаем объект device, измените на ваш /dev/input/event2
device = evdev.InputDevice('/dev/input/event2')

StatusGo = 0
StatusLR = 0

# Целевая скорость, робот наберет ее когда разгонится
speed = 100

# Реальная скорость, с нее робот стартует
real_speedB = 0
real_speedC = 0

speedB = 0
speedC = 0

# Признак зарершения программы (нажат акнопка Start на джойстике)
STOP = False

# Создаем объекты - моторы B и C
B = LargeMotor('outB')
C = LargeMotor('outC')

# Цикл пока не нажата кнопка Start на джойстике
while not STOP:
    # Читаем список событий с джойстика
    gen = device.read()
    try:
       # Для всех событий в списке
       for event in gen:
           # Выделяем те, которые возникли при нажатиях кнопок
           if event.type == evdev.ecodes.EV_KEY:
               # преобразуем такие события в строку myStr
               myStr = str(event)
               
               # если отпущена кнопка "Влево"
               if myStr.find("code 168, type 01, val 00") >= 0:
                   StatusLR = 0
               # если нажата кнопка "Влево"
               if myStr.find("code 168, type 01, val 01") >= 0:
                   StatusLR = -1
               # если удерживается кнопка "Влево"
               if myStr.find("code 168, type 01, val 02") >= 0:
                   StatusLR = -2
               # если отпущена кнопка "Вправо"
               if myStr.find("code 208, type 01, val 00") >= 0:
                   StatusLR = 0
               # если нажата кнопка "Вправо"
               if myStr.find("code 208, type 01, val 01") >= 0:
                   StatusLR = 1
               # если удерживается кнопка "Вправо"
               if myStr.find("code 208, type 01, val 02") >= 0:
                   StatusLR = 2
               # если отпущена кнопка "Вперед"  
               if myStr.find("code 172, type 01, val 00") >= 0:
                   StatusGo = 0
               # если нажата кнопка "Вперед"
               if myStr.find("code 172, type 01, val 01") >= 0:
                   StatusGo = speed*0.75
               # если удерживается кнопка "Вперед"
               if myStr.find("code 172, type 01, val 02") >= 0:
                   StatusGo = speed
               # если отпущена кнопка "Назад"
               if myStr.find("code 114, type 01, val 00") >= 0:
                   StatusGo = 0
               # если нажата кнопка "Назад"
               if myStr.find("code 114, type 01, val 01") >= 0:
                   StatusGo = -1*(speed * 0.75)
               # если удерживается кнопка "Назад"
               if myStr.find("code 114, type 01, val 02") >= 0:
                   #print ("GO BREAK")
                   StatusGo = -1*speed
               # если нажата кнопка "Start"         
               if myStr.find("code 164, type 01, val 02") >= 0:
                   print ("BREAK! STOP PROGRAMM")
                   STOP = True
               # Кнопка - среднее значение скорости
               if myStr.find("code 164, type 01, val 01") >= 0:
                   speed = 75
               # Кнопка нажата - уменьшить скорость
               if myStr.find("code 115, type 01, val 01") >= 0:
                   speed = speed - 5
                   if(speed < 5):
                       speed = 5
               # Кнопка удерживается - уменьшить скорость        
               if myStr.find("code 115, type 01, val 02") >= 0:
                   speed = speed - 1
                   if(speed < 5): 
                       speed = 5
               # Кнопка нажата - увеличить скорость
               if myStr.find("code 113, type 01, val 01") >= 0:
                   speed = speed + 5
                   if(speed > 100): 
                       speed = 100
               # Кнопка удерживается - увеличить скорость
               if myStr.find("code 113, type 01, val 02") >= 0:
                   speed = speed + 1
                   if(speed > 100): 
                       speed = 100

    except IOError:
        pass
    
    # перебрасываем статусы нажатий в мощности моторов  
    speedB = StatusGo
    speedC = StatusGo
    
    # Поворот влево
    if(StatusLR < 0):
        speedB = speedB-(25*abs(StatusLR))
        speedC = speedC+(25*abs(StatusLR))         
    # поворот вправо
    if(StatusLR > 0):
        speedC = speedC-(25*StatusLR)
        speedB = speedB+(25*StatusLR)
    
    # ограничение скорости
    if(speedB > 100):
        speedB = 100
    if(speedC > 100):
        speedC = 100
    if(speedB < -100):
        speedB = -100
    if(speedC < -100):
        speedC = -100

    # плавный разгон и торможение
    if(abs(speedB) > 5 and abs(speedC) > 5):
        real_speedB = real_speedB*0.95 + speedB*0.05
        real_speedC = real_speedC*0.95 + speedC*0.05
    if(speedB == 0 and speedC == 0):
        real_speedB = real_speedB*0.95
        real_speedC = real_speedC*0.95
    if(speedB == 0 and abs(real_speedB) < 5):
        real_speedB = 0
    if(speedC == 0 and abs(real_speedC) < 5):
        real_speedC = 0
    
    # подаем рассчитанные мощности на моторы        
    B.run_forever(speed_sp=real_speedB*9)
    C.run_forever(speed_sp=real_speedC*9)

# останавливаем моторы после вылета из цикла         
B.stop(stop_action="hold")
C.stop(stop_action="hold")

# сигнал завершения программы
Sound.beep()