RaspberryPi Mousceをジョイスティックで操作してみる

前回、RaspberryPi mouceのセットアップについてまとめました。↓

Raspberry mouceのセットアップ方法（ubuntu,デバイスドライバー,ROS）

https://denkenmusic.com/raspberry-mouceのセットアップ方法（ubuntuデバイスドライバーros

　ROSの勉強をする為に、アールティ社のRaspberry Mouceを購入しました。価格は6万強しましたので正直躊躇しましたが、買ってしまえば勉強せざるを得ない状況に追い込まれるので、逆に良かったとも思っています。さて、ROSを勉強するつもりでRaspberry mouceを買った訳ですが、何から進めればいいのかよく分からないので、下記の本を参考にしました。↓Raspberry Piで学ぶROSロボット入門　しかし、けっこう前の本なのでのOSのバージョン等が古く、そのまま進めても上手く動作しませんでした。（モーターが動かない。。）自分のセットア...

今回はRT社の公式サイトで公開されているサンプルプログラムを応用し、JoyC（m5stickcのやつ）でRaspberryPi Mouceのモーターを無線で動かしてみたいと思います。（ROSではありません。）

1 はじめに
2 サンプルプログラム(Python)
3 RaspberryPi Mouce側
4 JoyC M5stickC側
5 UDPでMouce制御

はじめに

サンプルプログラムでは、PCのキーボード入力でモーターを制御するものがシェルスクリプト、C、pythonで用意されています。

初めはこれをbluetoothコントローラー(PS3のコントローラーなど)で制御させてみようと思っていたのですが、ubuntu20.04では無理っぽい？ので、WIFIで制御する事にしました。

プログラムはpythonで記述することにしました。

サンプルプログラム(Python)

filename_motoren = “/dev/rtmotoren0”
filename_motor_r = “/dev/rtmotor_raw_r0”
filename_motor_l = “/dev/rtmotor_raw_l0”
filename_motor = “/dev/rtmotor0”

このインスタンスを使うとモーターが動くようです。

下記関数で制御しています。

def motor_drive(freq_l=”0″, freq_r=”0″):
with open(filename_motor_l, ‘w’) as f:
f.write(freq_l)
with open(filename_motor_r, ‘w’) as f:
f.write(freq_r)

引数に入る数字でスピード、回転方向を決めているようです。

これを元に、WIFIで無線操作出来るようプログラムを組んでみました。

RaspberryPi Mouce側

import socket
import time
import _thread

filename_motoren = "/dev/rtmotoren0"
filename_motor_r = "/dev/rtmotor_raw_r0"
filename_motor_l = "/dev/rtmotor_raw_l0"
filename_motor = "/dev/rtmotor0"

#SPEED = "400"
#import network

#wlan = network.WLAN(network.STA_IF)
#wlan.active(True)
#if not wlan.isconnected():
#    print('connecting to network...')
#    wlan.connect('SSID', 'PASS')
#    while not wlan.isconnected():
#        pass
#    print('network config:', wlan.ifconfig())

HOST = "IP"
PORT = ポート番号

msg = "nane"
msgt = "nane"
s = socket.socket(socket.AF_INET,socket.SOCK_DGRAM)
s.setsockopt(socket.SOL_SOCKET,socket.SO_REUSEADDR,True)
s.bind((HOST, PORT))

def motor_drive(freq_l="0", freq_r="0"):
    with open(filename_motor_l, 'w') as f:
        f.write(freq_l)
    with open(filename_motor_r, 'w') as f:
        f.write(freq_r)

print("Motor On")
with open(filename_motoren, 'w') as f:
    f.write("1")
time.sleep(0.5)
#s = socket(AF_INET, SOCK_DGRAM)
#s.bind((HOST, PORT))
#msg = "none"

def buttonA_wasPressed():
    print("Motor OFF")
    with open(filename_motoren, 'w') as f:
        f.write("0")
    time.sleep(0.5)  # global params

#def ledb(bt):
#    while(bt == 1):

def recieve():
    while(True):
        global msg
        msg, address = s.recvfrom(4096)
    #print(f"message: {msg}\nfrom: {address}")
        msgt = msg.decode()
        if "buzzer" or "buzzeroff" not in  msgt:
            msg = msgt
        #msg = int(msg)
        time.sleep(0.01)
    #print(msg)
_thread.start_new_thread(recieve, ())

while(True):
    if msg == "B": # in msg:
        #if msg > 120:
        #msgA = msg[1:]
        #msgA = int(msgA)
        #if msgA > 120:
        #    print(msgA)
        motor_drive("-400", "-400")
        print(msg)
    elif msg == "F": #msgA < 80:
        #    print(msgA)
        motor_drive("400", "400")
        print(msg)
#else:
        #    motor_drive("0", "0")
    elif msg == "S": # in msg:
        #msgB = msg[1:]
        #msgB = int(msgB)
        motor_drive("0", "0")
        print(msg)
    elif msg == "l":
        #if msgB > 120:
            #motor_drive(msgB)
            #msgBr = msgB * 2
            #msgBl = -msgB * 2
        motor_drive("-400", "400")
        print(msg)
    elif msg == "r":
         #   print(msgB)
            #msgBr = -msgB * 2
            #msgBl = msgB * 2
          #  motor elif msg == "S":_drive("-400", "-400")
       # else:
        motor_drive("400", "-400")
        print(msg)
    elif msg == "lt":
        motor_drive("0", "400")
        print(msg)
    elif msg == "rt":
        motor_drive("400", "0")
        print(msg)
    elif msg == "blt":
        motor_drive("0", "-400")
        print(msg)
    elif msg == "brt":
        motor_drive("-400", "0")
        print(msg)
    if msgt == "buzzer":
        with open('/dev/rtbuzzer0', 'w') as f:
            f.write(freq_dict["k"])
        ledb()
    if msgt == "buzzeroff":
        with open('/dev/rtbuzzer0', 'w') as f:
            f.write(freq_dict["0"])
   # else:
     #   motor_drive("0", "0")
    time.sleep(0.01)

s.close()

100

101

102

103

104

105

106

107

108

109

110

111

112

113

114

115

116

117

118

119

120

121

122

123

124

125

import socket

import time

import _thread

filename_motoren = "/dev/rtmotoren0"

filename_motor_r = "/dev/rtmotor_raw_r0"

filename_motor_l = "/dev/rtmotor_raw_l0"

filename_motor = "/dev/rtmotor0"

#SPEED = "400"

#import network

#wlan = network.WLAN(network.STA_IF)

#wlan.active(True)

#if not wlan.isconnected():

# print('connecting to network...')

# wlan.connect('SSID', 'PASS')

# while not wlan.isconnected():

# pass

# print('network config:', wlan.ifconfig())

HOST = "IP"

PORT = ポート番号

msg = "nane"

msgt = "nane"

s = socket.socket(socket.AF_INET,socket.SOCK_DGRAM)

s.setsockopt(socket.SOL_SOCKET,socket.SO_REUSEADDR,True)

s.bind((HOST, PORT))

def motor_drive(freq_l="0", freq_r="0"):

with open(filename_motor_l, 'w') as f:

f.write(freq_l)

with open(filename_motor_r, 'w') as f:

f.write(freq_r)

print("Motor On")

with open(filename_motoren, 'w') as f:

f.write("1")

time.sleep(0.5)

#s = socket(AF_INET, SOCK_DGRAM)

#s.bind((HOST, PORT))

#msg = "none"

def buttonA_wasPressed():

print("Motor OFF")

with open(filename_motoren, 'w') as f:

f.write("0")

time.sleep(0.5) # global params

#def ledb(bt):

# while(bt == 1):

def recieve():

while(True):

global msg

msg, address = s.recvfrom(4096)

#print(f"message: {msg}\nfrom: {address}")

msgt = msg.decode()

if "buzzer" or "buzzeroff" not in msgt:

msg = msgt

#msg = int(msg)

time.sleep(0.01)

#print(msg)

_thread.start_new_thread(recieve, ())

while(True):

if msg == "B": # in msg:

#if msg > 120:

#msgA = msg[1:]

#msgA = int(msgA)

#if msgA > 120:

# print(msgA)

motor_drive("-400", "-400")

print(msg)

elif msg == "F": #msgA < 80:

# print(msgA)

motor_drive("400", "400")

print(msg)

#else:

# motor_drive("0", "0")

elif msg == "S": # in msg:

#msgB = msg[1:]

#msgB = int(msgB)

motor_drive("0", "0")

print(msg)

elif msg == "l":

#if msgB > 120:

#motor_drive(msgB)

#msgBr = msgB * 2

#msgBl = -msgB * 2

motor_drive("-400", "400")

print(msg)

elif msg == "r":

# print(msgB)

#msgBr = -msgB * 2

#msgBl = msgB * 2

# motor elif msg == "S":_drive("-400", "-400")

# else:

motor_drive("400", "-400")

print(msg)

elif msg == "lt":

motor_drive("0", "400")

print(msg)

elif msg == "rt":

motor_drive("400", "0")

print(msg)

elif msg == "blt":

motor_drive("0", "-400")

print(msg)

elif msg == "brt":

motor_drive("-400", "0")

print(msg)

if msgt == "buzzer":

with open('/dev/rtbuzzer0', 'w') as f:

f.write(freq_dict["k"])

ledb()

if msgt == "buzzeroff":

with open('/dev/rtbuzzer0', 'w') as f:

f.write(freq_dict["0"])

# else:

# motor_drive("0", "0")

time.sleep(0.01)

s.close()

JoyC M5stickC側

import network
from socket import socket, AF_INET, SOCK_DGRAM
from m5stack import *
from m5ui import *
from uiflow import *
import hat
import time

setScreenColor(0x111111)

wlan = network.WLAN(network.STA_IF)
wlan.active(True)
if not wlan.isconnected():
    print('connecting to network...')
    wlan.connect('SSID', 'PASS')
    while not wlan.isconnected():
        pass
    print('network config:', wlan.ifconfig())
 
serv_address = ("IP", ポート番号)
s = socket(AF_INET, SOCK_DGRAM)

hat_joyc0 = hat.get(hat.JOYC)

message = "S"
message2 = "name"

def buttonA_wasPressed():
    message = "off"
    print(message)
 
while True:
    #y = hat_joyc0.GetX(0)
    x = hat_joyc0.GetY(0)
    
    y = hat_joyc0.GetX(1)
    #xx = hat_joyc0.GetY(1)
    
    print(x)
    print(y)#
    #x = str(x)
    #y = str(y)
    if x > 120 and 120 > y > 80:
        message = "F"
        print(message)
    elif x < 80 and 120 > y > 80:
        print(message)
        message = "B"
    elif 120 > x > 80 and y > 120:
        print(message)
        message = "l"
    elif 120 > x > 80 and y < 80:
        print(message)
        message = "r"
    elif x > 120 and y > 120:
        message = "lt"
        print(message)
    elif x > 120 and y < 80:
        message = "rt"
        print(message)
    elif x < 80 and y > 120:
        message = "blt"
        print(message)
    elif x < 80 and y < 80:
        message = "brt"
        print(message)
    elif 120 > y > 80 and 120 > x > 80:
        message = "S"
        print(message)
    if hat_joyc0.GetPress(1):
        message2 = "buzzer"
        print(message2)
    if hat_joyc0.GetPress(0):
        message2 = "buzzeroff"
        print(message2)
    btnA.wasPressed(buttonA_wasPressed)
    #print(x)
    #print(y)
    
    #message = "C" + x
    #message2 = "B" + y
    s.sendto(message.encode("utf-8"), serv_address)
    s.sendto(message2.encode("utf-8"), serv_address)
    #s.sendto(message2.encode("utf-8"), serv_address)
               
    time.sleep(0.1)

import network

from socket import socket, AF_INET, SOCK_DGRAM

from m5stack import *

from m5ui import *

from uiflow import *

import hat

import time

setScreenColor(0x111111)

wlan = network.WLAN(network.STA_IF)

wlan.active(True)

if not wlan.isconnected():

print('connecting to network...')

wlan.connect('SSID', 'PASS')

while not wlan.isconnected():

pass

print('network config:', wlan.ifconfig())

serv_address = ("IP", ポート番号)

s = socket(AF_INET, SOCK_DGRAM)

hat_joyc0 = hat.get(hat.JOYC)

message = "S"

message2 = "name"

def buttonA_wasPressed():

message = "off"

print(message)

while True:

#y = hat_joyc0.GetX(0)

x = hat_joyc0.GetY(0)

y = hat_joyc0.GetX(1)

#xx = hat_joyc0.GetY(1)

print(x)

print(y)#

#x = str(x)

#y = str(y)

if x > 120 and 120 > y > 80:

message = "F"

print(message)

elif x < 80 and 120 > y > 80:

print(message)

message = "B"

elif 120 > x > 80 and y > 120:

print(message)

message = "l"

elif 120 > x > 80 and y < 80:

print(message)

message = "r"

elif x > 120 and y > 120:

message = "lt"

print(message)

elif x > 120 and y < 80:

message = "rt"

print(message)

elif x < 80 and y > 120:

message = "blt"

print(message)

elif x < 80 and y < 80:

message = "brt"

print(message)

elif 120 > y > 80 and 120 > x > 80:

message = "S"

print(message)

if hat_joyc0.GetPress(1):

message2 = "buzzer"

print(message2)

if hat_joyc0.GetPress(0):

message2 = "buzzeroff"

print(message2)

btnA.wasPressed(buttonA_wasPressed)

#print(x)

#print(y)

#message = "C" + x

#message2 = "B" + y

s.sendto(message.encode("utf-8"), serv_address)

s.sendto(message2.encode("utf-8"), serv_address)

#s.sendto(message2.encode("utf-8"), serv_address)

time.sleep(0.1)

UDPでMouce制御

ubuntuの最新バージョンではPS3のコントローラーが無線で接続出来なかったので、m5stickCで動かしてみた pic.twitter.com/W8Xwz24x2A
— ロビヲ (@ketunorobio) September 9, 2022

次は、距離センサで障害物をよけるやつをやってみます。

はじめに

サンプルプログラム(Python)

RaspberryPi Mouce側

JoyC M5stickC側

UDPでMouce制御

コメントを残す コメントをキャンセル

コメントを残すコメントをキャンセル