Raspberry pi でロボットアームを動かす その６ ゲームパッドで動かせるようにする
微調整の際に役立ちそうなので、ゲームパッドでアームを動かせるようにしておきます。
また、激しく揺れるので、MDFにネジ止めしました。
動作確認
$ lsusb Bus 001 Device 006: ID 046d:082c Logitech, Inc. Bus 001 Device 005: ID 0079:0011 DragonRise Inc. Gamepad Bus 001 Device 003: ID 0424:ec00 Standard Microsystems Corp. SMSC9512/9514 Fast Ethernet Adapter Bus 001 Device 002: ID 0424:9514 Standard Microsystems Corp. Bus 001 Device 001: ID 1d6b:0002 Linux Foundation 2.0 root hub
DragonRise Inc. Gamepad と認識されています。
スクリプトその１ pygame
以下を使用させていただきました。
ドキュメントはこっち。 pygame.joystick — Pygame v1.9.2 documentation
SSHから起動すると pygame.error: video system not initialized と吐きますので、リモートデスクトップから起動しました。
動作チェック
で、実際に触ってみたら・・・
x and y : 0.999969482422 , 0.0 x and y : 0.0 , 0.0
と出ました。
押すのと、離すのとで、アクションです。
ボタンは、y, x, b, a, L, R, SELECT, START が、 0～7 に対応していました。
thread
ここでつまりました。
thread 作成が出来ないわけでもないし、動かないわけでもないのですが、なにか、main thread の影響がサブスレッドに出ているような挙動をします。
pygameのドキュメントlをちゃんと読めば分かるのかもしれませんが、pygame必須ではないので、別の方法を探ります。
コーディングその２ evdev
libpython-dev をインストールしてから、
$ pip install evdev
私の場合は、０にWEBカメラが刺さってますので、１を使用しています。
#!/usr/bin/python #coding: utf-8 import evdev devices = [evdev.InputDevice(fn) for fn in evdev.list_devices()] for device in devices: print(device.fn, device.name, device.phys) device = evdev.InputDevice('/dev/input/event1') print(device) for event in device.read_loop(): if event.type == evdev.ecodes.EV_KEY: print(evdev.util.categorize(event))
実行
$ ./evdev_test.py ('/dev/input/event0', 'HD Webcam C615', 'usb-3f980000.usb-1.4/button') ('/dev/input/event1', 'USB Gamepad ', 'usb-3f980000.usb-1.2/input0') device /dev/input/event1, name "USB Gamepad ", phys "usb-3f980000.usb-1.2/input0" key event at 1485825399.515801, 288 (['BTN_JOYSTICK', 'BTN_TRIGGER']), down key event at 1485825399.667815, 288 (['BTN_JOYSTICK', 'BTN_TRIGGER']), up key event at 1485825399.891846, 289 (BTN_THUMB), down key event at 1485825400.019861, 289 (BTN_THUMB), up key event at 1485825400.235895, 290 (BTN_THUMB2), down key event at 1485825400.371904, 290 (BTN_THUMB2), up key event at 1485825400.587932, 291 (BTN_TOP), down key event at 1485825400.739953, 291 (BTN_TOP), up key event at 1485825402.404162, 292 (BTN_TOP2), down key event at 1485825402.468181, 292 (BTN_TOP2), up key event at 1485825402.796214, 293 (BTN_PINKIE), down key event at 1485825402.924227, 293 (BTN_PINKIE), up key event at 1485825403.364283, 294 (BTN_BASE), down key event at 1485825403.476296, 294 (BTN_BASE), up key event at 1485825403.732329, 295 (BTN_BASE2), down key event at 1485825403.884350, 295 (BTN_BASE2), up
扱いやす形式で得る。
#!/usr/bin/python #coding: utf-8 import sys import evdev devices = [evdev.InputDevice(fn) for fn in evdev.list_devices()] for device in devices: print(device.fn, device.name, device.phys) def main(): device = evdev.InputDevice('/dev/input/event1') print(device) for event in device.read_loop(): # event : code sec timestamp() type usec value if event.type == evdev.ecodes.EV_KEY: print("{} {} {}".format(event.timestamp(), event.code, event.value)) elif event.type == evdev.ecodes.EV_ABS: print("{} {} {}".format(event.timestamp(), event.code, event.value)) if __name__ == '__main__': try: main() except KeyboardInterrupt: pass print("\n") sys.exit()
スレッドテスト
テストした所、スレッドも普通に動いたようです。
コーディング
#!/usr/bin/python #coding: utf-8 import sys import time import threading import Adafruit_PCA9685 import evdev devices = [evdev.InputDevice(fn) for fn in evdev.list_devices()] for device in devices: print(device.fn, device.name, device.phys) class MyThread(threading.Thread): def __init__(self): threading.Thread.__init__(self) self.channel = 0 self.FLAG_MOVE = False self.DIRECTION = 0 # PCA9685 チャンネルに合わせる self.PWM_NUMS = [375,375,150,375,275] # 初期値 self.MINS = [150,200,150,150,260] # 最小値 self.MAXS = [600,500,600,600,380] # 最大値 self.pwm = Adafruit_PCA9685.PCA9685() self.pwm.set_pwm_freq(60) self.set2def() def set2def(self): self.pwm.set_pwm(0, 0, self.PWM_NUMS[0]) self.pwm.set_pwm(1, 0, self.PWM_NUMS[1]) self.pwm.set_pwm(2, 0, self.PWM_NUMS[2]) self.pwm.set_pwm(3, 0, self.PWM_NUMS[3]) self.pwm.set_pwm(4, 0, self.PWM_NUMS[4]) def run(self): print(" === start sub thread ===") while True: while self.FLAG_MOVE: if self.DIRECTION == 1 and self.PWM_NUMS[self.channel] < self.MAXS[self.channel]: self.PWM_NUMS[self.channel] += 1 elif self.DIRECTION == -1 and self.PWM_NUMS[self.channel] > self.MINS[self.channel]: self.PWM_NUMS[self.channel] -= 1 self.pwm.set_pwm(self.channel, 0, self.PWM_NUMS[self.channel]) print("Thread {}. {} {}".format(self.channel, self.DIRECTION, self.PWM_NUMS[self.channel])) time.sleep(0.01) print(" === end sub thread ===") def main(): th = MyThread() th.setDaemon(True) th.start() device = evdev.InputDevice('/dev/input/event1') print(device) for event in device.read_loop(): # event : code sec timestamp() type usec value if event.type == evdev.ecodes.EV_KEY: print("{} {} {}".format(event.timestamp(), event.code, event.value)) if event.code == 290: # Bボタン th.channel = 2 # MG996R 傾き：上 if event.value == 0: th.FLAG_MOVE = False th.DIRECTION = 0 elif event.value == 1: th.FLAG_MOVE = True th.DIRECTION = -1 elif event.code == 291: # Aボタン th.channel = 2 # MG996R 傾き：上 if event.value == 0: th.FLAG_MOVE = False th.DIRECTION = 0 elif event.value == 1: th.FLAG_MOVE = True th.DIRECTION = 1 elif event.code == 292: # Lボタン th.channel = 3 # MG996R 手首旋回 if event.value == 0: th.FLAG_MOVE = False th.DIRECTION = 0 elif event.value == 1: th.FLAG_MOVE = True th.DIRECTION = -1 elif event.code == 293: # Rボタン th.channel = 3 # MG996R 手首旋回 if event.value == 0: th.FLAG_MOVE = False th.DIRECTION = 0 elif event.value == 1: th.FLAG_MOVE = True th.DIRECTION = 1 elif event.code == 288: # Xボタン th.channel = 4 # MG996R ハンド if event.value == 0: th.FLAG_MOVE = False th.DIRECTION = 0 elif event.value == 1: th.FLAG_MOVE = True th.DIRECTION = 1 elif event.code == 289: # Yボタン th.channel = 4 # MG996R ハンド if event.value == 0: th.FLAG_MOVE = False th.DIRECTION = 0 elif event.value == 1: th.FLAG_MOVE = True th.DIRECTION = -1 elif event.type == evdev.ecodes.EV_ABS: print("{} {} {}".format(event.timestamp(), event.code, event.value)) if event.code == 0: # X軸,左右 th.channel = 0 # 旋回 if event.value == 0: th.FLAG_MOVE = True th.DIRECTION = 1 elif event.value == 127: th.FLAG_MOVE = False th.DIRECTION = 0 elif event.value == 255: th.FLAG_MOVE = True th.DIRECTION = -1 elif event.code == 1: # y軸,上下 th.channel = 1 # 傾き：下 if event.value == 0: th.FLAG_MOVE = True th.DIRECTION = 1 elif event.value == 127: th.FLAG_MOVE = False th.DIRECTION = 0 elif event.value == 255: th.FLAG_MOVE = True th.DIRECTION = -1 if __name__ == '__main__': try: main() except KeyboardInterrupt: pass print("\n") sys.exit()
なお、このままですと、サブスレッドの終了には到達しません。
また、多軸を同時に制御できるようにもしていません。
ちょっと改善？
パッドの入力を同時に複数箇所受け取れるようにします。
はじめ、軸ごとにスレッドを立ててみたのですが、変に動きが遅かったので、なんかかっこ悪いですが、スレッドは１つで、一回のループの中で５つのモータのフラグを順繰りで読み取り制御させるようにしました。
なので、正確には同時に２箇所の制御はしておらず、高速に順番に動かしているだけです。
#!/usr/bin/python #coding: utf-8 import sys import time import threading import Adafruit_PCA9685 import evdev devices = [evdev.InputDevice(fn) for fn in evdev.list_devices()] for device in devices: print(device.fn, device.name, device.phys) class MyThread(threading.Thread): def __init__(self): threading.Thread.__init__(self) # PCA9685 チャンネルに合わせる self.CHANNELS = [0,1,2,3,4] self.FLAG_MOVES = [False,False,False,False,False] self.DIRECTIONS = [0,0,0,0,0] self.PWM_NUMS = [375,375,150,375,275] # 初期値 self.MINS = [150,200,150,150,260] # 最小値 self.MAXS = [600,500,600,600,380] # 最大値 self.pwm = Adafruit_PCA9685.PCA9685() self.pwm.set_pwm_freq(60) self.set2def() def set2def(self): self.pwm.set_pwm(0, 0, self.PWM_NUMS[0]) self.pwm.set_pwm(1, 0, self.PWM_NUMS[1]) self.pwm.set_pwm(2, 0, self.PWM_NUMS[2]) self.pwm.set_pwm(3, 0, self.PWM_NUMS[3]) self.pwm.set_pwm(4, 0, self.PWM_NUMS[4]) def run(self): print("MyThread start.") while True: for channel in self.CHANNELS: if self.FLAG_MOVES[channel] and self.DIRECTIONS[channel] == 1 and self.PWM_NUMS[channel] < self.MAXS[channel]: self.PWM_NUMS[channel] += 1 elif self.FLAG_MOVES[channel] and self.DIRECTIONS[channel] == -1 and self.PWM_NUMS[channel] > self.MINS[channel]: self.PWM_NUMS[channel] -= 1 self.pwm.set_pwm(channel, 0, self.PWM_NUMS[channel]) # print("Thread:moves {}. {} {}".format(channel, self.DIRECTIONS[channel], self.PWM_NUMS[channel])) # time.sleep(0.01) print("MyThread end.") def main(): th = MyThread() th.setDaemon(True) th.start() device = evdev.InputDevice('/dev/input/event0') print(device) for event in device.read_loop(): # event : code sec timestamp() type usec value if event.type == evdev.ecodes.EV_KEY: print("{} {} {}".format(event.timestamp(), event.code, event.value)) if event.code == 290: # Bボタン channel = 2 # MG996R 傾き：上 if event.value == 0: th.FLAG_MOVES[channel] = False th.DIRECTIONS[channel] = 0 elif event.value == 1: th.FLAG_MOVES[channel] = True th.DIRECTIONS[channel] = -1 elif event.code == 291: # Aボタン channel = 2 # MG996R 傾き：上 if event.value == 0: th.FLAG_MOVES[channel] = False th.DIRECTIONS[channel] = 0 elif event.value == 1: th.FLAG_MOVES[channel] = True th.DIRECTIONS[channel] = 1 elif event.code == 292: # Lボタン channel = 3 # MG996R 手首旋回 if event.value == 0: th.FLAG_MOVES[channel] = False th.DIRECTIONS[channel] = 0 elif event.value == 1: th.FLAG_MOVES[channel] = True th.DIRECTIONS[channel] = -1 elif event.code == 293: # Rボタン channel = 3 # MG996R 手首旋回 if event.value == 0: th.FLAG_MOVES[channel] = False th.DIRECTIONS[channel] = 0 elif event.value == 1: th.FLAG_MOVES[channel] = True th.DIRECTIONS[channel] = 1 elif event.code == 288: # Xボタン channel = 4 # MG996R ハンド if event.value == 0: th.FLAG_MOVES[channel] = False th.DIRECTIONS[channel] = 0 elif event.value == 1: th.FLAG_MOVES[channel] = True th.DIRECTIONS[channel] = 1 elif event.code == 289: # Yボタン channel = 4 # MG996R ハンド if event.value == 0: th.FLAG_MOVES[channel] = False th.DIRECTIONS[channel] = 0 elif event.value == 1: th.FLAG_MOVES[channel] = True th.DIRECTIONS[channel] = -1 elif event.type == evdev.ecodes.EV_ABS: print("{} {} {}".format(event.timestamp(), event.code, event.value)) if event.code == 0: # X軸,左右 channel = 0 # 旋回 if event.value == 0: th.FLAG_MOVES[channel] = True th.DIRECTIONS[channel] = 1 elif event.value == 127: th.FLAG_MOVES[channel] = False th.DIRECTIONS[channel] = 0 elif event.value == 255: th.FLAG_MOVES[channel] = True th.DIRECTIONS[channel] = -1 elif event.code == 1: # y軸,上下 channel = 1 # 傾き：下 if event.value == 0: th.FLAG_MOVES[channel] = True th.DIRECTIONS[channel] = 1 elif event.value == 127: th.FLAG_MOVES[channel] = False th.DIRECTIONS[channel] = 0 elif event.value == 255: th.FLAG_MOVES[channel] = True th.DIRECTIONS[channel] = -1 if __name__ == '__main__': try: main() except KeyboardInterrupt: pass print("\n") sys.exit()
