読者です 読者をやめる 読者になる 読者になる

Raspberry Pi 備忘録 / Mbedもあるよ!

Raspberry Pi であれこれやった事の記録

Raspberry pi でロボットアームを動かす その6 ゲームパッドで動かせるようにする

微調整の際に役立ちそうなので、ゲームパッドでアームを動かせるようにしておきます。

f:id:pongsuke:20170131151032j:plain

また、激しく揺れるので、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 と認識されています。

スクリプトその1 pygame

以下を使用させていただきました。

Joystickの入力を取得する - 強火で進め

ドキュメントはこっち。 pygame.joystick — Pygame v1.9.2 documentation

SSHから起動すると pygame.error: video system not initialized と吐きますので、リモートデスクトップから起動しました。

動作チェック

www2.elecom.co.jp

で、実際に触ってみたら・・・

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必須ではないので、別の方法を探ります。

コーディングその2 evdev

libpython-dev をインストールしてから、

$ pip install evdev

Quick Start — Python-evdev

私の場合は、0にWEBカメラが刺さってますので、1を使用しています。

#!/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()

なお、このままですと、サブスレッドの終了には到達しません。

また、多軸を同時に制御できるようにもしていません。

ちょっと改善?

パッドの入力を同時に複数箇所受け取れるようにします。

はじめ、軸ごとにスレッドを立ててみたのですが、変に動きが遅かったので、なんかかっこ悪いですが、スレッドは1つで、一回のループの中で5つのモータのフラグを順繰りで読み取り制御させるようにしました。

なので、正確には同時に2箇所の制御はしておらず、高速に順番に動かしているだけです。

#!/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()