Bluetoothリモコンでロボット操作「PS3のジョイコントローラーでRaspberry Piを制御」python

前回、PS3のBluetoothジョイコンをRaspberry Pi にペアリングする方法についてまとめました。

今回は、ジョイコンのBluetooth信号を受け取り、サーボモーターを駆動させてロボットを操作するプログラムを作りたいと思います。

ロボットにつきましては、過去記事でまとめましたものをベースに使いたいと思います。↓

unitv2で2足歩行ロボットの作成「uartでRaspberryPi ZERO Wのi2cを制御」

https://denkenmusic.com/unitv2で2足歩行ロボットの作成「uartでraspberrypiのi2cを制御」

前回unitv2からuart通信にてraspberrypiのi2cを制御しサーボモーターを駆動させました。↓　今回は最終目的である「2足歩行ロボット」を完成させたいと思います。前回まででunitv2→uart→raspberrypizerow→i2c→pca9685→サーボモーターの形が出来ましたので、あとは「ロボットの骨組みを作る」「モーションの作成」で完了します。では初めに、ロボットの骨組みを作成します。　ダイソーの¥１００化粧箱これにデバイス全て突っ込むhttps://twitter.com/ketunorobio/status/1443164099395788803本当は3Dプリンターで作りたいのですが、まだ...

動作の流れとしては、

python(raspberry pi)→i2c→pca9685→サーボモーター

となります。

ブラウザで制御する時よりもプログラムの数がはるかに少ないので（１つでOk）、その面ではスマートで良いなと思いました。

はじめに、ハマった所をまとめておこうと思います。

1 ボタンの入力待ちが発生するのでwhileでボタン長押し判定ができない
- 1.1 threadで改善
2 入力値について
3 プログラム
- 3.1 python
4 動作確認

ボタンの入力待ちが発生するのでwhileでボタン長押し判定ができない

threadで改善

Bluetoothコントローラーのボタン長押し検知が中々難しい。コントローラーの入力待ちで強制待機になってしまうからループ処理出来ない。
Threadでやれば上手くいくかな#Python #ラズパイ
— ロビヲ (@ketunorobio) November 11, 2021

ボタン入力の値受け取りをループで回せば、下のような例でボタン長押し判定が可能です。

while True:

　if a == fw:

　　while a == fw:

　　　サーボ駆動のプログラム

　　　a = ボタン入力を受け取るプログラム

　elif….

　a = ボタン入力を受け取るプログラム

しかし、厄介な事にボタン入力を受け取るプログラムの時に「待ち」が発生してしまうようで、連続した動作が出来ないようになっていました。

これを解決してくれたのが「threading」のライブラリでした。

threadingは、名前の通り並列処理が可能になりますので、つまり「ボタン入力用のループ」と「サーボ駆動用のループ」を同時に走らせる事が出来ます。

したがって、ボタン入力の待ちが発生しようが、一度ボタンの値をglobalの変数に格納してしまえば、それの通りにサーボ駆動用のループで動き続けてくれます。

入力値について

各ボタンの入力値は下記サイトに詳しく乗っています。↓

ラズパイでラジコン(3)：Pythonでdualshock3の入力を読み取る

しかし、ボタンを表す識別「num」の値が、私の場合は異なりました。理由は分かりませんが、一概にみんな上記サイトと同じ値にはならないかもしれません。

自分で調べて、値を確認する事が重要です。

プログラム

python

import struct
import time
import threading
import Adafruit_PCA9685

pwm = Adafruit_PCA9685.PCA9685()
pwm.set_pwm_freq(60)

p = 400
i = 395
n = 375
t = 362

pwm.set_pwm(0, 0, p)
pwm.set_pwm(2, 0, i)
pwm.set_pwm(1, 0, n)
pwm.set_pwm(3, 0, t)

device_path = "/dev/input/js0"

# unsigned long, short, unsigned char, unsigned char
EVENT_FORMAT = "LhBB";
EVENT_SIZE = struct.calcsize(EVENT_FORMAT)

mode = "z"

while True:
    try:
        with open(device_path, "rb") as device:
            event = device.read(EVENT_SIZE)
        print("ok")
        break
    except IOError:
        con = False

def control():
    global mode
    with open(device_path, "rb") as device:
        event = device.read(EVENT_SIZE)
        while event:
            (ds3_time, ds3_val, ds3_type, ds3_num) = struct.unpack(EVENT_FORMAT, event)
            print( "{0}, {1}, {2}, {3}".format( ds3_time, ds3_val, ds3_type, ds3_num ) )
            if ds3_type == 1:
                if ds3_num == 0 and ds3_val == 1:
                    print("X")
                elif ds3_num == 1 and ds3_val == 1:
                    print("〇")
                elif ds3_num == 2 and ds3_val == 1:
                    print("△")
                elif ds3_num == 3 and ds3_val == 1:
                    print("□")
                elif ds3_num == 13 and ds3_val == 1:
                    print("上")
                    mode = "a"
                    print(mode)
                elif ds3_num == 14 and ds3_val == 1:
                    print("下")
                    mode = "b"
                elif ds3_num == 15 and ds3_val == 1:
                    print("左")
                    mode = "c"
                elif ds3_num == 16 and ds3_val == 1:
                    print("右")
                    mode = "d"
                elif ds3_type == 1 and ds3_val == 0:
                    print("st")
                    mode = "s"
            elif ds3_type == 2:
                if ds3_num == 0 and ds3_val < -1000:
                    print("hidari")
                    mode = "cc"
                elif ds3_num == 1 and ds3_val < -1000:
                    print("ue")
                    mode = "aa"
                elif ds3_num == 0 and ds3_val > 2000:
                    print("migi")
                    mode = "dd"
                elif ds3_num == 1 and ds3_val > 2000:
                    print("sita")
                    mode == "bb"
                elif ds3_type == 2 and ds3_val ==0:
                    print("stop")
                    mode = "s"
            event = device.read(EVENT_SIZE)
    #time.sleep(1)
#control()
# Parallel Process Command
th = threading.Thread(target=control)
th.start()

while True:
    #global mode
    if mode == "a":
        print("aaaaaaaaa")
        while(n <= 500):
            pwm.set_pwm(3, 0, t)
            pwm.set_pwm(1, 0, n)
            time.sleep(0.0005)
            n = n + 1
            if t <= 450:
                t = t + 1
        while(i <= 495):
            pwm.set_pwm(2, 0, i)
            time.sleep(0.0005)
            i = i + 1
        while(n >= 375):
            pwm.set_pwm(3, 0, t)
            pwm.set_pwm(1, 0, n)
            time.sleep(0.0005)
            n = n - 1
            if t >= 362:
                t = t - 1
        while(i >= 395):
            pwm.set_pwm(2, 0, i)
            time.sleep(0.0005)
            i = i - 1
        while(t >= 250):
            pwm.set_pwm(3, 0, t)
            pwm.set_pwm(1, 0, n)
            time.sleep(0.0005)
            t = t - 1
            if n >= 300:
                n = n - 1
        while(p >= 300):
            pwm.set_pwm(0, 0, p)
            time.sleep(0.0005)
            p = p - 1
        while(t <= 362):
            pwm.set_pwm(3, 0, t)
            pwm.set_pwm(1, 0, n)
            time.sleep(0.0005)
            t = t + 1
            if n <= 375:
                n = n + 1
        while(p <= 400):
            pwm.set_pwm(0, 0, p)
            time.sleep(0.0005)
            p = p + 1
        #if mode !="a":
            #break
    elif mode == "b":
        print("bbbbbbbbbb")
        while(n <= 500):
            pwm.set_pwm(3, 0, t)
            pwm.set_pwm(1, 0, n)
            time.sleep(0.0005)
            n = n + 1
            if t <= 450:
                t = t + 1
        while(i >= 295):
            pwm.set_pwm(2, 0, i)
            time.sleep(0.0005)
            i = i - 1
        while(n >= 375):
            pwm.set_pwm(3, 0, t)
            pwm.set_pwm(1, 0, n)
            time.sleep(0.0005)
            n = n - 1
            if t >= 363:
                t = t - 1
        while(i <= 395):
            pwm.set_pwm(2, 0, i)
            time.sleep(0.0005)
            i = i + 1
        while(t >= 250):
            pwm.set_pwm(3, 0, t)
            pwm.set_pwm(1, 0, n)
            time.sleep(0.0005)
            t = t - 1
            if n >= 300:
                n = n - 1
        while(p <= 500):
            pwm.set_pwm(0, 0, p)
            time.sleep(0.0005)
            p = p + 1
        while(t <= 363):
            pwm.set_pwm(3, 0, t)
            pwm.set_pwm(1, 0, n)
            time.sleep(0.0005)
            t = t + 1
            if n <= 375:
                n = n + 1
        while(p >= 400):
            pwm.set_pwm(0, 0, p)
            time.sleep(0.0005)
            p = p - 1
        #if mode != "b":
            #break
    elif mode == "c":
        print("cccccccccc")
        while(t >= 250):
            pwm.set_pwm(3, 0, t)
            pwm.set_pwm(1, 0, n)
            time.sleep(0.0005)
            t = t - 1
            if n >= 300:
                n = n - 1
        while(p <= 500):
            pwm.set_pwm(0, 0, p)
            time.sleep(0.0005)
            p = p + 1
        while(t <= 363):
            pwm.set_pwm(3, 0, t)
            pwm.set_pwm(1, 0, n)
            time.sleep(0.0005)
            t = t + 1
            if n <= 375:
                n = n + 1
        while(p >= 400):
            pwm.set_pwm(0, 0, p)
            time.sleep(0.0005)
            p = p - 1
        #if mode != "c":
            #break
    elif mode == "d":
        print("dddddddddd")
        while(n <= 500):
            pwm.set_pwm(3, 0, t)
            pwm.set_pwm(1, 0, n)
            time.sleep(0.0005)
            n = n + 1
            if t <= 450:
                t = t + 1
        while(i >= 295):
            pwm.set_pwm(2, 0, i)
            time.sleep(0.0005)
            i = i - 1
        while(n >= 375):
            pwm.set_pwm(3, 0, t)
            pwm.set_pwm(1, 0, n)
            time.sleep(0.0005)
            n = n - 1
            if t >= 363:
                t = t - 1
        while(i <= 395):
            pwm.set_pwm(2, 0, i)
            time.sleep(0.0005)
            i = i + 1
        #if mode != "d":
            #break
    elif mode == "aa":
        print("AAAAAAAAAAAA")
    elif mode == "bb":
        print("BBBBBBBBBBB")
    elif mode == "cc":
        print("CCCCCCCCCCC")
    elif mode == "dd":
        print("DDDDDDDDD")
    elif mode == "s":
        print("ssssssssss")
    else:
        print("nasi")
    time.sleep(0.05)

th.join()

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

126

127

128

129

130

131

132

133

134

135

136

137

138

139

140

141

142

143

144

145

146

147

148

149

150

151

152

153

154

155

156

157

158

159

160

161

162

163

164

165

166

167

168

169

170

171

172

173

174

175

176

177

178

179

180

181

182

183

184

185

186

187

188

189

190

191

192

193

194

195

196

197

198

199

200

201

202

203

204

205

206

207

208

209

210

211

212

213

214

215

216

217

218

219

220

221

222

223

224

225

226

227

228

229

230

231

232

233

234

235

236

237

238

239

240

241

242

243

244

245

246

247

248

249

250

251

252

253

254

255

import struct

import time

import threading

import Adafruit_PCA9685

pwm = Adafruit_PCA9685.PCA9685()

pwm.set_pwm_freq(60)

p = 400

i = 395

n = 375

t = 362

pwm.set_pwm(0, 0, p)

pwm.set_pwm(2, 0, i)

pwm.set_pwm(1, 0, n)

pwm.set_pwm(3, 0, t)

device_path = "/dev/input/js0"

# unsigned long, short, unsigned char, unsigned char

EVENT_FORMAT = "LhBB";

EVENT_SIZE = struct.calcsize(EVENT_FORMAT)

mode = "z"

while True:

try:

with open(device_path, "rb") as device:

event = device.read(EVENT_SIZE)

print("ok")

break

except IOError:

con = False

def control():

global mode

with open(device_path, "rb") as device:

event = device.read(EVENT_SIZE)

while event:

(ds3_time, ds3_val, ds3_type, ds3_num) = struct.unpack(EVENT_FORMAT, event)

print( "{0}, {1}, {2}, {3}".format( ds3_time, ds3_val, ds3_type, ds3_num ) )

if ds3_type == 1:

if ds3_num == 0 and ds3_val == 1:

print("X")

elif ds3_num == 1 and ds3_val == 1:

print("〇")

elif ds3_num == 2 and ds3_val == 1:

print("△")

elif ds3_num == 3 and ds3_val == 1:

print("□")

elif ds3_num == 13 and ds3_val == 1:

print("上")

mode = "a"

print(mode)

elif ds3_num == 14 and ds3_val == 1:

print("下")

mode = "b"

elif ds3_num == 15 and ds3_val == 1:

print("左")

mode = "c"

elif ds3_num == 16 and ds3_val == 1:

print("右")

mode = "d"

elif ds3_type == 1 and ds3_val == 0:

print("st")

mode = "s"

elif ds3_type == 2:

if ds3_num == 0 and ds3_val < -1000:

print("hidari")

mode = "cc"

elif ds3_num == 1 and ds3_val < -1000:

print("ue")

mode = "aa"

elif ds3_num == 0 and ds3_val > 2000:

print("migi")

mode = "dd"

elif ds3_num == 1 and ds3_val > 2000:

print("sita")

mode == "bb"

elif ds3_type == 2 and ds3_val ==0:

print("stop")

mode = "s"

event = device.read(EVENT_SIZE)

#time.sleep(1)

#control()

# Parallel Process Command

th = threading.Thread(target=control)

th.start()

while True:

#global mode

if mode == "a":

print("aaaaaaaaa")

while(n <= 500):

pwm.set_pwm(3, 0, t)

pwm.set_pwm(1, 0, n)

time.sleep(0.0005)

n = n + 1

if t <= 450:

t = t + 1

while(i <= 495):

pwm.set_pwm(2, 0, i)

time.sleep(0.0005)

i = i + 1

while(n >= 375):

pwm.set_pwm(3, 0, t)

pwm.set_pwm(1, 0, n)

time.sleep(0.0005)

n = n - 1

if t >= 362:

t = t - 1

while(i >= 395):

pwm.set_pwm(2, 0, i)

time.sleep(0.0005)

i = i - 1

while(t >= 250):

pwm.set_pwm(3, 0, t)

pwm.set_pwm(1, 0, n)

time.sleep(0.0005)

t = t - 1

if n >= 300:

n = n - 1

while(p >= 300):

pwm.set_pwm(0, 0, p)

time.sleep(0.0005)

p = p - 1

while(t <= 362):

pwm.set_pwm(3, 0, t)

pwm.set_pwm(1, 0, n)

time.sleep(0.0005)

t = t + 1

if n <= 375:

n = n + 1

while(p <= 400):

pwm.set_pwm(0, 0, p)

time.sleep(0.0005)

p = p + 1

#if mode !="a":

#break

elif mode == "b":

print("bbbbbbbbbb")

while(n <= 500):

pwm.set_pwm(3, 0, t)

pwm.set_pwm(1, 0, n)

time.sleep(0.0005)

n = n + 1

if t <= 450:

t = t + 1

while(i >= 295):

pwm.set_pwm(2, 0, i)

time.sleep(0.0005)

i = i - 1

while(n >= 375):

pwm.set_pwm(3, 0, t)

pwm.set_pwm(1, 0, n)

time.sleep(0.0005)

n = n - 1

if t >= 363:

t = t - 1

while(i <= 395):

pwm.set_pwm(2, 0, i)

time.sleep(0.0005)

i = i + 1

while(t >= 250):

pwm.set_pwm(3, 0, t)

pwm.set_pwm(1, 0, n)

time.sleep(0.0005)

t = t - 1

if n >= 300:

n = n - 1

while(p <= 500):

pwm.set_pwm(0, 0, p)

time.sleep(0.0005)

p = p + 1

while(t <= 363):

pwm.set_pwm(3, 0, t)

pwm.set_pwm(1, 0, n)

time.sleep(0.0005)

t = t + 1

if n <= 375:

n = n + 1

while(p >= 400):

pwm.set_pwm(0, 0, p)

time.sleep(0.0005)

p = p - 1

#if mode != "b":

#break

elif mode == "c":

print("cccccccccc")

while(t >= 250):

pwm.set_pwm(3, 0, t)

pwm.set_pwm(1, 0, n)

time.sleep(0.0005)

t = t - 1

if n >= 300:

n = n - 1

while(p <= 500):

pwm.set_pwm(0, 0, p)

time.sleep(0.0005)

p = p + 1

while(t <= 363):

pwm.set_pwm(3, 0, t)

pwm.set_pwm(1, 0, n)

time.sleep(0.0005)

t = t + 1

if n <= 375:

n = n + 1

while(p >= 400):

pwm.set_pwm(0, 0, p)

time.sleep(0.0005)

p = p - 1

#if mode != "c":

#break

elif mode == "d":

print("dddddddddd")

while(n <= 500):

pwm.set_pwm(3, 0, t)

pwm.set_pwm(1, 0, n)

time.sleep(0.0005)

n = n + 1

if t <= 450:

t = t + 1

while(i >= 295):

pwm.set_pwm(2, 0, i)

time.sleep(0.0005)

i = i - 1

while(n >= 375):

pwm.set_pwm(3, 0, t)

pwm.set_pwm(1, 0, n)

time.sleep(0.0005)

n = n - 1

if t >= 363:

t = t - 1

while(i <= 395):

pwm.set_pwm(2, 0, i)

time.sleep(0.0005)

i = i + 1

#if mode != "d":

#break

elif mode == "aa":

print("AAAAAAAAAAAA")

elif mode == "bb":

print("BBBBBBBBBBB")

elif mode == "cc":

print("CCCCCCCCCCC")

elif mode == "dd":

print("DDDDDDDDD")

elif mode == "s":

print("ssssssssss")

else:

print("nasi")

time.sleep(0.05)

th.join()

/dev/input/js0　にボタンの入力値が入ります。device.read(EVENT_SIZE　で入力された値を読み取り、struct.unpack(EVENT_FORMAT, event)　で各変数に値の種別ごとに割り振って格納しています。

後は前途した通り、上段のループでボタン入力値を受け取り、下段のループでは上段のループで得た値を利用してサーボモーターを駆動させています。

threading.Threadの引数、target=〇〇のように、〇〇の部分に関数を埋め込んでやれば、それが並列処理されます。

動作確認

https://twitter.com/ketunorobio/status/1459026079297900546

とりあえず十字キーだけですが、うまく動いてくれました。

近いうちに、アナログコントローラーでメカナムホイール操作もやってみようかと思います。

ボタンの入力待ちが発生するのでwhileでボタン長押し判定ができない

threadで改善

入力値について

プログラム

python

動作確認

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

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