1
0

Delete article

Deleted articles cannot be recovered.

Draft of this article would be also deleted.

Are you sure you want to delete this article?

@6186milktea

RaspiでWiiリモコン IRカメラ＆ヌンチャク対応

1
Posted at

調べると、cwiid等のライブラリが作られているものの、それらは古い環境でしか動かず、現環境で動かそうとすると面倒だ。そこでWiiリモコンとの通信を自前で実装することにした。

基本的に全部ここに載っていること。解析勢に感謝。

サンプルコード

ちゃっぴーとgeminiさんに手伝ってもらっているのでやや一貫性が無い部分が。
動けばよかろう。

動作環境はRaspi4。PCでも使えるはずだが、権限周りをどうにかしなくてはならない？未検証。

サンプル 
#edited
import socket
import time
from dataclasses import dataclass, field

PSM_CONTROL = 17
PSM_INTERRUPT = 19

debugMsg=False

@dataclass
class WiiBtn:
    A: bool = False
    B: bool = False
    PLUS: bool = False
    MINUS: bool = False
    HOME: bool = False
    UP: bool = False
    DOWN: bool = False
    LEFT: bool = False
    RIGHT: bool = False
    ONE: bool = False
    TWO: bool = False
@dataclass
class WiiAcc:
    x: int = 0
    y: int = 0
    z: int = 0
@dataclass
class WiiIR:
    x:int = 0
    y:int = 0
@dataclass
class Nunchuk:
    stick_x:int = 0
    stick_y:int = 0
    acc_x:int = 0
    acc_y:int = 0
    acc_z:int = 0
    btn_c:bool = False
    btn_z:bool = False
@dataclass
class Wiistate:
    WiiBtn: "WiiBtn" = field(default_factory=WiiBtn)
    WiiAcc: "WiiAcc" = field(default_factory=WiiAcc)
    WiiIR: "WiiIR" = field(default_factory=WiiIR)
    Nunchuk: "Nunchuk" = field(default_factory=Nunchuk)
    
    def __str__(self):
        return (
            f"A:{self.WiiBtn.A} "
            f"B:{self.WiiBtn.B} "
            f"L:{self.WiiBtn.LEFT} "
            f"R:{self.WiiBtn.RIGHT} "
            f"U:{self.WiiBtn.UP} "
            f"D:{self.WiiBtn.DOWN} | "
            f"ACC({self.WiiAcc.x},{self.WiiAcc.y},{self.WiiAcc.z}) | "
            f"IR({self.WiiIR.x},{self.WiiIR.y}) | "
            f"NUN({self.Nunchuk.stick_x},{self.Nunchuk.stick_y},{self.Nunchuk.btn_c},{self.Nunchuk.btn_z})"
        )

def _write_wii_reg(ctl, address, data):
    msg = bytearray([0x52, 0x16, 0x04]) # 0x04: Register write
    msg.append((address >> 16) & 0xFF)
    msg.append((address >> 8) & 0xFF)
    msg.append(address & 0xFF)    
    size = len(data)
    msg.append(size)
    msg.extend(data)
    msg.extend([0] * (16 - size)) # Padding to 16 bytes
    ctl.send(msg)
    time.sleep(0.05) # 指定の50ms待機

def _setup_wii_full(address):
    if debugMsg:print(f"Connecting to {address}...")
    ctl = socket.socket(socket.AF_BLUETOOTH, socket.SOCK_SEQPACKET, socket.BTPROTO_L2CAP)
    intr = socket.socket(socket.AF_BLUETOOTH, socket.SOCK_SEQPACKET, socket.BTPROTO_L2CAP)
    ctl.connect((address, PSM_CONTROL))
    intr.connect((address, PSM_INTERRUPT))
    if debugMsg:print("Connected")
    # Ex init
    if debugMsg:print("EX init start")
    _write_wii_reg(ctl, 0xa400f0, [0x55])
    time.sleep(0.05)
    _write_wii_reg(ctl, 0xa400fb, [0x00])
    time.sleep(0.05)
    if debugMsg:print("EX init end")
    # IR init
    # 1.IR (0x06: Clock + Logic)
    if debugMsg:print("IR init start")
    ctl.send(bytes([0x52, 0x13, 0x06]))
    time.sleep(0.05)
    ctl.send(bytes([0x52, 0x1a, 0x06]))
    time.sleep(0.05)
    # 2.IR ON (0x01)
    _write_wii_reg(ctl, 0xb00030, [0x01])
    # 3.Sensitivity Level 3 (Marcan's suggest)
    _write_wii_reg(ctl, 0xb00000, [0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x90, 0x00, 0xC0])
    _write_wii_reg(ctl, 0xb0001a, [0x40, 0x00])
    # 4.IRmode (0x01: Basic, 0x03: Extended)
    _write_wii_reg(ctl, 0xb00033, [0x01]) # <- Basic:10byte,Extend:12byte
    # 5.reIR ON (0x08)
    _write_wii_reg(ctl, 0xb00030, [0x08])
    if debugMsg:print("IR init end")
    time.sleep(0.2)
    # repoteMode:0x37 (Btn+Acc+IR(10)+Ex(6))
    if debugMsg:print("mode init start")
    time.sleep(0.2)
    ctl.send(bytes([0x52, 0x12, 0x04, 0x37]))
    time.sleep(0.2)
    if debugMsg:print("mode init end")
    
    return ctl, intr

def _parse_ir_extended(data):
    dots = []
    # Dot 1 (data[7, 8, 9])
    d1_x = data[7] | ((data[9] & 0x30) << 4)
    d1_y = data[8] | ((data[9] & 0xC0) << 2)
    # Dot 2 (data[10, 11, 9])
    d2_x = data[10] | ((data[9] & 0x03) << 8)
    d2_y = data[11] | ((data[9] & 0x0C) << 6)
    # Dot 3 (data[12, 13, 14])
    d3_x = data[12] | ((data[14] & 0x30) << 4)
    d3_y = data[13] | ((data[14] & 0xC0) << 2)
    # Dot 4 (data[15, 16, 14])
    d4_x = data[15] | ((data[14] & 0x03) << 8)
    d4_y = data[16] | ((data[14] & 0x0C) << 6)

    for x, y in [(d1_x, d1_y), (d2_x, d2_y), (d3_x, d3_y), (d4_x, d4_y)]:
        dots.append((x, y)) #suggest::ignore p(1023,1023)
    return dots

def startWiimote(WII_MAC):
    global _ctl,_intr
    _ctl, _intr = _setup_wii_full(WII_MAC)
    _intr.setblocking(False)

def readWiimote():
    
    state=Wiistate()
    try:
        packet = _intr.recv(1024)

        if len(packet) >= 19 : #and packet[1] == 0x37
            # Btn,Acc
            state.WiiBtn.LEFT=bool(packet[2] & 0x01)
            state.WiiBtn.RIGHT=bool(packet[2] & 0x02)
            state.WiiBtn.DOWN=bool(packet[2] & 0x04)
            state.WiiBtn.UP=bool(packet[2] & 0x08)
            state.WiiBtn.PLUS=bool(packet[2] & 0x10)
            state.WiiBtn.TWO=bool(packet[3] & 0x01)
            state.WiiBtn.ONE=bool(packet[3] & 0x02)
            state.WiiBtn.B=bool(packet[3] & 0x04)
            state.WiiBtn.A=bool(packet[3] & 0x08)
            state.WiiBtn.MINUS=bool(packet[3] & 0x10)
            state.WiiBtn.HOME=bool(packet[3] & 0x80)
            state.WiiAcc.x=(packet[4] << 2) | ((packet[2] & 0x60) >> 5)
            state.WiiAcc.y=(packet[5] << 2) | ((packet[3] & 0x20) >> 4)
            state.WiiAcc.z=(packet[6] << 2) | ((packet[3] & 0x40) >> 5)
            # IR
            dots = _parse_ir_extended(packet)
            state.WiiIR.x=dots[0][0]
            state.WiiIR.y=dots[0][1]
            # Ex nunchuck
            ext = packet[17:23]
            if debugMsg:print(len(packet))
            state.Nunchuk.stick_x= ext[0]
            state.Nunchuk.stick_y = ext[1]
            state.Nunchuk.acc_x = ext[2]
            state.Nunchuk.acc_y = ext[3]
            state.Nunchuk.acc_z = ext[4]
            btn = ext[5]
            state.Nunchuk.btn_c = not (btn & 0x02)
            state.Nunchuk.btn_z = not (btn & 0x01)
            
            if debugMsg:print(state)
        else:
            pass
        
    except BlockingIOError:
        return
    
    return state

if __name__=="__main__":
    WII_MAC="00:25:A0:27:93:C5" #myBlackWiiremote
    startWiimote(WII_MAC)
    mywii=Wiistate()
    while True:
        state = readWiimote()

        if state is not None:
            mywii = state

        if mywii:
            print(mywii)    #IRあってる？

WII_MACには使うWiiリモコンのMACアドレスを入れる。
reportMode:0x37,拡張コントローラー:ヌンチャクでしか書いてないが、先述のwikiをもとに改変すればいろいろ対応できるはず。拡張コントローラーを挿していないと0埋めになってしまう模様。

補足

reportMODE

reportMODEの種類は以下。
モード (ID)　構成内容　
0x30　ボタンのみ。 最も軽量。
0x31　ボタン + 加速度。
0x33　ボタン + 加速度 + IR(12byte)。 4点すべての赤外線座標を送る。
0x35　ボタン + 加速度 + 拡張(16byte)。 MotionPlusのジャイロを送る。
0x37　ボタン + 加速度 + IR(10byte) + 拡張(6byte)。
0x3e,0x3f 特殊　2回で一セットらしい

0x33と0x37とでIRのデータ量が違うが、これはIR起動時に指定する、IRモードと合わせておく必要がある(詳細後述)。これは沼ポイント。

IRカメラの起動

ここがやや不思議な挙動をするので注意。Wiiリモコンのロット、WiimotionPlus版などによって若干挙動が変わりうるようだ。
IRmodeはreportModeと対応する必要があり、ずれているとデータが送信されなかったり止まったりこれまた変な挙動をしてしまう。

0x01: Basicは10byte分送信、0x37と対応。
0x03: Extendは12byte分送信、0x33と対応。

ちなみにどの辺がExtendなのかというと、ポインターの概算サイズが追加されるらしい。大まかな距離感が測れる...というものか。

reportの解釈

reportModeによってやや構成が異なる。
なぜか綺麗にブロックごとにわかれておらず、Accなどは一部の桁がBtnのブロックに交じって居たりする。用参照。

(再掲)

その他 参考

分解の様子が丁寧に残されている。メンテが必要な際には参考。

ログ

26-12-26 ちまちま情報収集　IR以外はだいたい成功？　しばらく保留期間
26-02-17 とりかかる　成功
26-02-19 記事制作(仮)

1
0
0

Register as a new user and use Qiita more conveniently

  1. You get articles that match your needs
  2. You can efficiently read back useful information
  3. You can use dark theme
What you can do with signing up
Sign upLogin
1
0

Delete article

Deleted articles cannot be recovered.

Draft of this article would be also deleted.

Are you sure you want to delete this article?