EV3をラズパイのゲームパッドからインターネット経由で操縦してみた。（その4）

今日も前回の続きです。

前回のEV3のTRACK3Rにはカメラを搭載していませんでした。そこでカメラを搭載してみますが、ただ載せるのではなく回転台の上にWebカメラを設置することでコントローラーからカメラの方向を自由に回転できるように工夫してみます。なお、Webカメラはこの記事同様にラズパイゼロでmjpg-streamerを走らせてブロードキャストしています。

また、MQTTの通信量を減らして少しでも遅延の解消ができるように、スクリプトにも手を加えます。方法としては、何もコントローラを操作していないときはメッセージをPublisherであるRaspberry Pi（以降ラズパイ）から送信しないようにします。

カメラの台座の回転については、ゲームパッドの左側前面にある上下のボタンを押して操作することにします。上ボタンを押すと右回り、下ボタンを押すと左回りというように逆向きの回転ができるようにします。

早速Pythonコードです。まずはPublisherであるラズパイ側から。

#!/usr/bin/env python

import pygame, sys, time
from pygame.locals import *
import paho.mqtt.client as mqtt

# Initialize a joystick and pygame
pygame.init()
pygame.joystick.init()
joystick = pygame.joystick.Joystick(0)
joystick.init()
screen = pygame.display.set_mode((400,300))
pygame.display.set_caption('Controll ev3')

interval = 0.01

# This is the Publisher
loopQuit = False

str_js_cash = "0,0,0,0,0" #一つ前のメッセージの内容を記憶するためのキャッシュ

while loopQuit == False:

        axis_1 = joystick.get_axis(1)
        axis_1 = axis_1 * 100
        str_axis_1 = str(int(axis_1))

        axis_3 = joystick.get_axis(3)
        axis_3 = axis_3 * 100
        str_axis_3 = str(int(axis_3))

        button_5 = str(joystick.get_button(5)*100)
        
        button_4 = str(joystick.get_button(4)*25) #カメラの回転用
        button_6 = str(joystick.get_button(6)*-25)　#カメラの逆回転用

        str_js = str_axis_3 + "," + str_axis_1 + "," + button_5 + "," + button_4 + "," + button_6
        
        if str_js_cash != "0,0,0,0,0": #一つ前のメッセージの内容が0,0,0,0,0でないなら以下を実行する。
                client = mqtt.Client()
                client.connect("localhost",1883,60)
                client.publish("topic/motor-BCAD/dt", str_js);
                client.disconnect();
                print (str_js)                      

        str_js_cash = str_js
                
        for event in pygame.event.get():
            if event.type == QUIT:
                loopQuit = True
            elif event.type == pygame.KEYDOWN:
                if event.key == pygame.K_ESCAPE:
                    loopQuit = True
        
        time.sleep(interval)

pygame.quit()
sys.exit()

カメラ台の回転用にボタンの4、6を追加し、遅延解消のために一つ前のメッセージをキャッシュしてフィルタリングすることで、無駄なメッセージの送信を抑えています。

次にSubscriberであるEV3側のコードです。

#!/usr/bin/env python
import paho.mqtt.client as mqtt
from ev3dev.auto import *

# This is the Subscriber
mb = Motor(OUTPUT_B)
mc = Motor(OUTPUT_C)
ma = MediumMotor(OUTPUT_A)
md = MediumMotor(OUTPUT_D) #カメラ回転台用のモーターを追加

def on_connect(client, userdata, flags, rc):
    print("Connected with result code "+str(rc))
    client.subscribe("topic/motor-BCA/dt")

def on_message(client, userdata, msg):
    #print(msg.payload) #メッセージを表示していると遅延の原因になりそうなのでコメントアウト
    msg_array = msg.payload.split(",")
    mb.duty_cycle_sp = msg_array[0]
    mc.duty_cycle_sp = msg_array[1]
    ma.duty_cycle_sp = msg_array[2]
    MDdcsp = int(msg_array[3]) + int(msg_array[4]) #カメラ回転台のボタンの数値を足すことで制御
    md.duty_cycle_sp = str(MDdcsp)

client = mqtt.Client()
client.connect("PublisherであるラズパイのIPアドレス",1883,60)

client.on_connect = on_connect
client.on_message = on_message

mb.run_direct()
mb.duty_cycle_sp = 0

mc.run_direct()
mc.duty_cycle_sp = 0

ma.run_direct()
ma.duty_cycle_sp = 0

md.run_direct()
md.duty_cycle_sp = 0

client.loop_forever()

ちょっとした工夫としては、カメラ回転台のボタンの値の制御は単純に二つのボタンの値を足すことで行っています。片方のボタンだけを押せばその値になりますし、両方のボタンを同時に押せばプラスマイナス0になり回転台は静止したままになります。

カメラが回転するところを除けば、結果としては一見この記事と同じことをやってはいますが、BluetoothではなくMQTTプロトコルを使いインターネット経由で操作している点が異なります。

今日のところはこの辺で。☕️

関連記事：「EV3をラズパイのゲームパッドからインターネット経由で操縦してみた。（その３）」