Игрушка ГАЗ-66 на пульте управления. Часть 2

Игрушка ГАЗ-66 на пульте управления. Часть 2 - 1

В этой части поговорим о программной составляющей, как оживлялась машинка. Какая ОС использовались, какой язык был выбран, с какими проблемами сталкивался.

1. Как работает в 2-х словах

Система состоит из сервера который установлен на машинке, и клиента который установлен на пульт. Сервер поднимает wifi точку доступа и ждет пока клиент не подключится. Сервер выполняет команды клиента, а так же передает на него видео с камеры.

2. ОС

Теперь поговорим об используемых операционных системах.
Поскольку вся система базируется на Raspberry pi 3, то и использовалась официальная ОС под неё. На момент создания последней версией была Stretch, она была и выбрана для использования на машинке и пульте управления. Но оказалось, что в ней есть баг(промучился неделю) из-за которого невозможно поднять wifi точку доступа. Поэтому для поднятия точки доступа была взята предыдущая версия Jessie не имевшая таких проблем.

Статья как поднять точку доступа. ^[1] Очень подробная, делал все по ней.

Пульт автоматически подключается к машинке, когда она поднимает точку доступа.
Автоматическое подключение к нашей точке, в файл /etc/network/interfaces добавить:

`auto wlan0
iface wlan0 inet dhcp 
wpa-ssid {ssid}
wpa-psk  {password}
`

2. Язык

Выбрал python потому что легко и просто.

3. Сервер

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

Сервер состоит из 2-х частей. Видео сервера и сервера управления.

3.1 Видео сервер

Было 2 варианта, как работать с видео камерой. 1-ый использовать модуль picamera и 2-ой использовать ПО mjpg-streamer. Долго не думая я решил использовать их оба, а какой именно использовать вынести в настройки конфига.

`
if conf.conf.VideoServerType == 'm' :
    cmd = "cd /home/pi/projects/mjpg-streamer-experimental && "
    cmd += './mjpg_streamer -o "./output_http.so -p {0} -w ./www" -i "./input_raspicam.so -x {1} -y {2} -fps 25 -ex auto -awb auto -vs -ISO 10"'.format(conf.conf.videoServerPort, conf.conf.VideoWidth, conf.conf.VideoHeight)

    print(cmd)
    os.system(cmd)

else :
    with picamera.PiCamera(resolution = str(conf.conf.VideoWidth) + 'x' + str(conf.conf.VideoHeight) , framerate = conf.conf.VideoRate) as Camera:
        output = camera.StreamingOutput()
        camera.output = output
        Camera.start_recording(output, format = 'mjpeg')
        try:
            address = (conf.conf.ServerIP, conf.conf.videoServerPort)
            server = camera.StreamingServer(address, camera.StreamingHandler)
            server.serve_forever()
        finally:
            Camera.stop_recording()
`

Поскольку они берут одни и теже настройки, работают они на одном и том же адресе. Нет ни каких проблем при коммуникации с пультом при переходе с одного на другой. Единственное, как мне кажется mjpg-streamer работает побыстрее.

3.2 Сервер управления

3.2.1 Взаимодействие между клиентом и сервером

Сервер и клиент обмениваются командами в виде json строк:

`{'type': 'remote', 'cmd': 'Start', 'status': True, 'val': 0.0}
{'type': 'remote', 'cmd': 'Y', 'status': True, 'val': 0.5}
{'type': 'remote', 'cmd': 'turn', 'x': 55, 'y': 32}
`

type — 'remote' или 'car' в зависимости от того кто шлет команду(клиент или сервер)
cmd — строка с именем кнопки, соответствующей имени кнопки на Game HAT, например:
- Start — кнопка Start
- Select — кнопка Select
- Y — кнопка Y
- и т.д.
- turn — команда изменения состояния джойстика, отвечает за поворот колес
status — True или False, в зависимости от того нажата кнопка или отжата. Событие о статусе кнопки отправляется каждый раз когда меняется ее состояние.
val — скорость и направление движения мотора от -1...1, значение типа float. Значащий параметр только для кнопок движения.
x — отклонение джойстика по оси x от -100...100, значение типа int
y — отклонение джойстика по оси y от -100...100, значение типа int

Дальше идет мой позор, переделать который руки не доходят. Машинка поднимает server socket и ожидает пока к ней не подключится клиент. При чем для каждого нового подключения она создает отдельный поток, и каждый новый клиент который будет подключаться к машинке сможет управлять ей )). Такого пока не может быть потому что, больше нет такого пульта ни у кого, и я поднимаю свою закрытую wifi сеть.

`def run(self): 
        TCP_IP = conf.conf.ServerIP
        TCP_PORT = conf.conf.controlServerPort
        BUFFER_SIZE = conf.conf.ServerBufferSize
        self.tcpServer = socket.socket(socket.AF_INET, socket.SOCK_STREAM) 
        self.tcpServer.setsockopt(socket.SOL_SOCKET, socket.SO_REUSEADDR, 1) 
        self.tcpServer.bind((TCP_IP, TCP_PORT)) 
        threads = [] 

        # Максимальное колличество подключений в очереди.
        self.tcpServer.listen(1)
        while True:
            print("Car server up : Waiting for connections from TCP clients...") 
            (conn, (ip, port)) = self.tcpServer.accept() 
            newthread = ClientThread(conn, ip, port) 
            newthread.start() 
            self.threads.append(newthread)      
`

3.2.2 Управление железом

При работе с Raspberry использовалась система нумерации выводов GPIO.BCM.

Управление светом осуществляется через gpio 17, оно соединено со 2-ым пином на L293. Далее каждый раз как приходит команда включить:

`GPIO.output(self.gpioLight, GPIO.HIGH)
`

или выключить:

`GPIO.output(self.gpioLight, GPIO.LOW)
`

вызываются соответствующие команды.

Управление сервоприводом происходит через плату PCA9685 по I2C шине, поэтому нужна соответствующая библиотека для неё Adafruit_PCA9685 ^[2]. PCA9685 подключена к серво через 7 pin. Необходимая частота ШИМ для работы с серво составляет 50 Герц или период 20 мс.
Принцип работы серво:

Игрушка ГАЗ-66 на пульте управления. Часть 2 - 2

При подаче сигнала длиной 1.5 мс колеса будут стоять по центру. При 1 мс. серво повернется максимально вправо, 2 мс. максимально влево. Поворотные кулаки в мостах на такие повороты не рассчитаны, поэтому угол поворота пришлось подбирать экспериментально.
Значения которые можно передавать в API Adafruit_PCA9685 составляют от 0..4095, 0 отсутствие сигнала, 4095 полное заполнение. Соответственно из этого диапазона нужно было подобрать значения подходящие для моих колес. Самое простое определить значения для ровно выставленных колес, это 1.5 мс перевести в значение из диапазона ~ 307.
Максимальное значение для права 245, для лева 369.

Значения приходящие от джойстика принимают значения от -100...100, поэтому их нужно было транслировать в диапазон от 245 до 369. Опять центр самое легкое, если 0 то это 307. Влево и вправо по формуле:

`val = int(HardwareSetting._turnCenter + (-1 * turn * HardwareSetting._turnDelta / HardwareSetting.yZero))
`

HardwareSetting._turnCenter — 307
turn — значение от джойстика от -100...100
HardwareSetting._turnDelta — 62, разница между центром и максимальным отклонением в сторону (307 — 245 = 62)
HardwareSetting.yZero — 100, максимальное значение получаемое от джойстика

Колеса прямо:

`def turnCenter(self):
        val = int(HardwareSetting._turnCenter)
        self.pwm_servo.set(val)
        CarStatus.statusCar['car']['turn'] = val
`

Поворот влево:

`def turnLeft(self, turn):
        val = int(HardwareSetting._turnCenter + (-1 * turn * HardwareSetting._turnDelta / HardwareSetting.yZero))
        self.pwm_servo.set(val)
        CarStatus.statusCar['car']['turn'] = val   
`

Поворот вправо:

`def turnRight(self, turn):
        val = int(HardwareSetting._turnCenter + (-1 * turn * HardwareSetting._turnDelta / HardwareSetting.yZero))
        self.pwm_servo.set(val)
        CarStatus.statusCar['car']['turn'] = val    
`

Управление двигателем происходит также через плату PCA9685 по I2C шине, поэтому используем Adafruit_PCA9685 ^[2]. Пины от 10 до 15 на PCA9685 подключены к L298N(использую на нем 2 канала, для увлечения мощности). 10 и 11 к ENA и ENB(наполняю их ШИМ-ом для регулирования скорости движения). 12, 13, 14, 15 к IN1, IN2, IN3, IN4 — отвечают за направление вращения мотора. Частота ШИМ здесь не особа важно, но я так же использую 50 Герц(мое значение по умолчанию).

Машинка стоит на месте:

`def stop(self):
        """
        Остановка мотора.
        """

        self.pwm.set_pwm(self.ena, 0, self.LOW)
        self.pwm.set_pwm(self.enb, 0, self.LOW)

        self.pwm.set_pwm(self.in1, 0, self.LOW)
        self.pwm.set_pwm(self.in4, 0, self.LOW)
        self.pwm.set_pwm(self.in2, 0, self.LOW)
        self.pwm.set_pwm(self.in3, 0, self.LOW)
`

Движение вперед:

`def back(self, speed):
        """
        Движение назад.

        Args:
            speed: Задаест скорость движение от 0 до 1.
        """

        self.pwm.set_pwm(self.ena, 0, int(speed * self.HIGH))
        self.pwm.set_pwm(self.enb, 0, int(speed * self.HIGH))

        self.pwm.set_pwm(self.in1, 0, self.LOW)
        self.pwm.set_pwm(self.in4, 0, self.LOW)
        self.pwm.set_pwm(self.in2, 0, self.HIGH)
        self.pwm.set_pwm(self.in3, 0, self.HIGH)   
`

Движение назад:

`def forward(self, speed):
        """
        Движение вперед.

        Args:
            speed: Задаест скорость движение от 0 до 1.
        """

        self.pwm.set_pwm(self.ena, 0, int(speed * self.HIGH))
        self.pwm.set_pwm(self.enb, 0, int(speed * self.HIGH))

        self.pwm.set_pwm(self.in1, 0, self.HIGH)
        self.pwm.set_pwm(self.in4, 0, self.HIGH)
        self.pwm.set_pwm(self.in2, 0, self.LOW)
        self.pwm.set_pwm(self.in3, 0, self.LOW)    
`

4. Клиент

4.1 Клавиатура

С ней были определенные проблемы, вначале мне хотелось сделать её событийной(заняло ~ 2 недели мучений). Но механические кнопки внесли свою лепту, дребежание контактов приводило к постоянным и непредсказуемым сработкам(алгоритмы борьбы придуманные мной работали неидеально). Затем мой коллега рассказал мне как сделаны клавиатуры. И я решил сделать так же, теперь опрашиваю состояние каждые 0.005 секунды(почему так, а кто его знает). И если оно изменилось посылаю значение на сервер.

`def run(self):
        try:
            while True:
                time.sleep(0.005)
                for pin in self.pins :
                    p = self.pins[pin]

                    status = p['status']

                    if GPIO.input(pin) == GPIO.HIGH :
                        p['status'] = False
                    else :
                        p['status'] = True

                    if p['status'] != status :
                        p['callback'](pin)

        except KeyboardInterrupt:
            GPIO.cleanup()
`

4.2 Джойстик

Чтение показаний происходит через плату ADS1115 по I2C шине, поэтому нужна соответствующая библиотека для неё Adafruit_PCA9685 ^[3]. Джойстик также подвержен дребезгу контактов, поэтому снимаю с него показания по аналогии с клавиатурой.

`def run(self):
        while True:
            X = self.adc.read_adc(0, gain=self.GAIN) / HardwareSetting.valueStep
            Y = self.adc.read_adc(1, gain=self.GAIN) / HardwareSetting.valueStep

            if X > HardwareSetting.xZero :
                X = X - HardwareSetting.xZero
            else :
                X = -1 * (HardwareSetting.xZero - X)

            if Y > HardwareSetting.yZero :
                Y = Y - HardwareSetting.yZero
            else :
                Y = -1 * (HardwareSetting.yZero - Y)

            if (abs(X) < 5) :
                X = 0
            if (abs(Y) < 5) :
                Y = 0

            if (abs(self.x - X) >= 1.0 or abs(self.y - Y) >= 1.0) :
                self.sendCmd(round(X), round(Y))

            self.x = X
            self.y = Y

            time.sleep(0.005)
`

При питании от 3.3 вольт диапазон значений которые выдает ADS1115 с джойстика от 0...26500. Привожу это к диапазону от -100...100. В моем диапазоне в районе 0 он всегда колеблется, поэтому если значения не превышают 5, то я считаю что это 0 (иначе будет флудить). Как только значения изменяются, посылаю их машинке.

4.3 Подключение к серверу управления

Коннект к серверу простая вещь:

`try :
    tcpClient = socket.socket(socket.AF_INET, socket.SOCK_STREAM)
    tcpClient.settimeout(2.0)
    tcpClient.connect((conf.conf.ServerIP, conf.conf.controlServerPort))
    self.signalDisplayPrint.emit("У+")
    carStatus.statusRemote['network']['control'] = True

    self.tcpClient = tcpClient
except socket.error as e:
    self.signalDisplayPrint.emit("У-")
    carStatus.statusRemote['network']['control'] = False

    time.sleep(conf.conf.timeRecconect)
    self.tcpClient = None

    continue        

if self.tcpClient :
    self.tcpClient.settimeout(None)
`

Но хочу обратить внимание на одну вещь. Если не использовать timeout в коннекте, то он может подвиснуть и придется ждать порядка пары минут(такое бывает когда клиент запустился раньше сервера). Решил это следующим способом, устанавливаю timeout на соединение. Как только соединение происходит, то убираю timeout.

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

4.4 Проверка подключения к WiFi

Проверяю состояние wifi, на предмет подключения к серверу. И если, что так же уведомляю себя о проблемах.

`def run(self):
        while True:
            time.sleep(1.0)

            self.ps = subprocess.Popen(['iwgetid'], stdout=subprocess.PIPE, stderr=subprocess.STDOUT)

            try:
                output = subprocess.check_output(('grep', 'ESSID'), stdin=self.ps.stdout)
                if re.search(r'djvu-car-pi3', str(output)) :
                    self.sendStatus('wifi+')
                    continue

            except subprocess.CalledProcessError:
                pass

            self.sendStatus('wifi-')

            self.ps.kill()
`

4.5 Подключение к видео серверу

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

Для отображения использовал:

`self.videoWidget = QVideoWidget()
`

И на него вывел:

`self.mediaPlayer = QMediaPlayer(None, QMediaPlayer.LowLatency)
self.mediaPlayer.setVideoOutput(self.videoWidget)
`

Подключение к потоковому видео:

`self.mediaPlayer.setMedia(QMediaContent(QUrl("http://{}:{}/?action=stream".format(conf.conf.ServerIP, conf.conf.videoServerPort))))
self.mediaPlayer.play()
`

Извиняюсь в очередной раз за тавтологию ). Контролирую состояние видео связи, на предмет подключения к видео серверу. И если, что так же уведомляю себя о проблемах.

Вот так выглядит когда все не работает:

remote

W — означает, что нет соединения с wifi
В — означает, что нет видео
У — означает, что нет управления

Иначе нет красных букв, идет видео с камеры. Фотку и видео с работой выложу в дальнейшем ) Надеюсь, что крепление для камеры придет в ближайшее время и я её наконец прикреплю нормально.