From bc3eca1e3bb547743e351fdaf1105a27be23cc2c Mon Sep 17 00:00:00 2001 From: klonfish Date: Mon, 26 Dec 2016 22:58:10 +0100 Subject: [PATCH] Added python3 version of wiiboard library and boad reading daemon --- wii-pair/daemon.py | 201 ++++++++++++++++++++++++++++++++++++ wii-pair/wiiboard.py | 241 +++++++++++++++++++++++++++++++++++++++++++ 2 files changed, 442 insertions(+) create mode 100755 wii-pair/daemon.py create mode 100644 wii-pair/wiiboard.py diff --git a/wii-pair/daemon.py b/wii-pair/daemon.py new file mode 100755 index 0000000..f538089 --- /dev/null +++ b/wii-pair/daemon.py @@ -0,0 +1,201 @@ +#!/usr/bin/env python3 +import sys +import os, math, random +import time +import socket +import struct +from threading import Thread +import bluetooth +import wiiboard + +known_boards = {'niklas': '00:26:59:34:C8:69', +'kaka': '00:26:59:37:04:15', +'michael': '00:24:44:65:5B:F8', +'fed': '00:23:CC:23:5E:1D', +} + +class StatusThread(Thread): + def __init__(self, t1, t2): + Thread.__init__(self) + self.t1 = t1 + self.t2 = t2 + self.stop = False + + def run(self): + t1 = self.t1 + t2 = self.t2 + colw = 50 + batw = colw - len('Bat: 000% || ') + while not self.stop: + print('') + print(('Left player - '+t1.addr).ljust(colw) + 'Right player - '+t2.addr) + print('-'*2*colw) + if t1.addr != 'dummy': + if t1.connected: + s1 = 'Connected' + bat = t1.board.battery + else: + s1 = 'Connecting...' + bat = -1 + else: + s1 = 'Dummy' + bat = 0.42 + px = 'Low ' if t1.pos_x is None else '%.3f'%t1.pos_x + py = 'Low ' if t1.pos_y is None else '%.3f'%t1.pos_y + p1 = 'Position: X: %s - Y: %s' % (px, py) + if bat is not None: + b1 = 'Bat: |%s| %03d%%' % (('='*int(batw*bat)).ljust(batw), int(bat*100)) + else: + b1 = 'Bat: |%s| ---%%' % ('?'*batw).ljust(batw) + w1 = 'Weight: %.1f' % t1.weight + + if t2.addr != 'dummy': + if t2.connected: + s2 = 'Connected' + bat = t2.board.battery + else: + s2 = 'Connecting...' + bat = -1 + else: + s2 = 'Dummy' + bat = 0.42 + px = 'Low ' if t2.pos_x is None else '%.3f'%t2.pos_x + py = 'Low ' if t2.pos_y is None else '%.3f'%t2.pos_y + p2 = 'Position: X: %s - Y: %s' % (px, py) + if bat is not None: + b2 = 'Bat: |%s| %03d%%' % (('='*int(batw*bat)).ljust(batw), int(bat*100)) + else: + b2 = 'Bat: |%s| ---%%' % ('?'*batw).ljust(batw) + w2 = 'Weight: %.1f' % t2.weight + + print(s1.ljust(colw) + s2) + print(b1.ljust(colw) + b2) + print(w1.ljust(colw) + w2) + print(p1.ljust(colw) + p2) + + time.sleep(1) + +class WiiThread(Thread): + def __init__(self, player, addr): + Thread.__init__(self) + self.addr = addr + self.player = player + self.stop = False + self.connected = False + self.board = None + self.pos_x = None + self.pos_y = None + self.weight = 0 + + def run(self): + if self.addr != 'dummy': + while not self.stop: + self.connected = False + self.pos = 'C' + self.board = wiiboard.Wiiboard() + print('%s: Connecting to %s' % (self.player, self.addr)) + try: + self.board.connect(self.addr) + except bluetooth.btcommon.BluetoothError: + continue + + time.sleep(1) + print('%s: Connected' % self.player) + self.connected = True + self.board.setLight(True) + + while not self.stop and self.board.status == 'Connected': + self.weight = self.board.mass.totalWeight + if self.board.mass.totalWeight > 15: + m = self.board.mass + x_balance = -(m.topLeft+m.bottomLeft) + (m.topRight+m.bottomRight) + x_balance = x_balance/float(m.totalWeight) + y_balance = -(m.bottomLeft+m.bottomRight) + (m.topLeft+m.topRight) + y_balance = y_balance/float(m.totalWeight) + self.pos_x = x_balance + self.pos_y = y_balance + else: + self.pos_x = None + self.pos_y = None + + time.sleep(0.1) + + self.board.disconnect() + else: + print('%s: Connected as DUMMY' % self.player) + self.connected = True + self.pos = 'D' + +wiis = [] +if len(sys.argv) > 1: + if sys.argv[1] in list(known_boards.keys()): + addr = known_boards[sys.argv[1]] + else: + addr = sys.argv[1] + t1 = WiiThread('L', addr) +else: + t1 = WiiThread('') + +t1.daemon = True +t1.start() + +if len(sys.argv) > 2: + if sys.argv[2] in list(known_boards.keys()): + addr = known_boards[sys.argv[2]] + else: + addr = sys.argv[2] + t2 = WiiThread('R', addr) +else: + t2 = WiiThread('') + +while not t1.connected: + time.sleep(0.1) + +t2.daemon = True +t2.start() + +while not t2.connected: + time.sleep(0.1) + +tStatus = StatusThread(t1, t2) +tStatus.daemon = True +tStatus.start() + +wiis = [t1, t2] + +s = socket.socket(socket.AF_INET, socket.SOCK_STREAM) +s.setsockopt(socket.SOL_SOCKET, socket.SO_REUSEADDR, 1) +s.bind(('', 4711)) +s.listen(1) + +try: + while True: + conn, addr = s.accept() + + while True: + m = conn.recv(1) + + if len(m) == 0: + conn.close() + break + + r = b'' + for w in wiis: + if w.pos_x is None: + v = -128 + else: + v = int(round(w.pos_x*100)) + r += struct.pack('b', v) + if w.pos_y is None: + v = -128 + else: + v = int(round(w.pos_y*100)) + r += struct.pack('b', v) + + conn.send(r) +except (KeyboardInterrupt, SystemExit): + t1.stop = True + t2.stop = True + tStatus.stop = True + sys.exit() + diff --git a/wii-pair/wiiboard.py b/wii-pair/wiiboard.py new file mode 100644 index 0000000..5e133b7 --- /dev/null +++ b/wii-pair/wiiboard.py @@ -0,0 +1,241 @@ +'''Wiiboard driver +Nedim Jackman December 2008 +No liability held for any use of this software. +More information at http://code.google.com/p/wiiboard-simple/ +''' + +import bluetooth +import sys +import _thread +import time +import binascii + +CONTINUOUS_REPORTING = 0x04 + +COMMAND_LIGHT = 0x11 +COMMAND_REPORTING = 0x12 +COMMAND_REQUEST_STATUS = 0x15 +COMMAND_REGISTER = 0x16 +COMMAND_READ_REGISTER = 0x17 + +#input is Wii device to host +INPUT_STATUS = 0x20 +INPUT_READ_DATA = 0x21 + +EXTENSION_8BYTES = 0x32 + +BUTTON_DOWN_MASK = 8 + +TOP_RIGHT = 0 +BOTTOM_RIGHT = 1 +TOP_LEFT = 2 +BOTTOM_LEFT = 3 + +BLUETOOTH_NAME = "Nintendo RVL-WBC-01" + + +class BoardEvent: + def __init__(self, topLeft,topRight,bottomLeft,bottomRight, buttonPressed, buttonReleased): + + self.topLeft = topLeft + self.topRight = topRight + self.bottomLeft = bottomLeft + self.bottomRight = bottomRight + self.buttonPressed = buttonPressed + self.buttonReleased = buttonReleased + #convenience value + self.totalWeight = topLeft + topRight + bottomLeft + bottomRight + +class Wiiboard: + def __init__(self): + self.datasocket = None + self.controlsocket = None + self.calibration = [] + self.calibrationRequested = False + self.LED = False + self.address = None + self.buttonDown = False + for i in range(3): + self.calibration.append([]) + for j in range(4): + self.calibration[i].append(10000) #high dummy value so events with it don't register + + self.status = "Disconnected" + self.lastEvent = BoardEvent(0,0,0,0,False,False) + self.mass = self.lastEvent + + try: + self.datasocket = bluetooth.BluetoothSocket(bluetooth.L2CAP) + self.datasocket.setttimeout(2) + self.controlsocket = bluetooth.BluetoothSocket(bluetooth.L2CAP) + self.controlsocket.setttimeout(2) + except ValueError: + raise Exception("Error: Bluetooth not found") + + def isConnected(self): + if self.status == "Connected": + return True + else: + return False + + # Connect to the Wiiboard at bluetooth address
+ def connect(self, address): + if address == None: + print("Non existant address") + return + self.datasocket.connect((address, 0x13)) + self.controlsocket.connect((address, 0x11)) + if self.datasocket and self.controlsocket: + print("Connected to Wiiboard at address " + address) + self.status = "Connected" + self.address = address + _thread.start_new_thread(self.receivethread, ()) + self.calibrate() + useExt = bytes([COMMAND_REGISTER, 0x04, 0xA4, 0x00, 0x40, 0x00]) + self.send(useExt) + self.setReportingType() + else: + print("Could not connect to Wiiboard at address " + address) + + # Disconnect from the Wiiboard + def disconnect(self): + if self.status == "Connected": + self.status = "Disconnecting" + while self.status == "Disconnecting": + time.sleep(0.001) + try: + self.datasocket.close() + self.controlsocket.close() + except: + pass + print("WiiBoard disconnected") + + # Try to discover a Wiiboard + def discover(self): + #print "Press the red sync button on the board now" + address = None + bluetoothdevices = bluetooth.discover_devices(duration = 6, lookup_names = True) + for addr, name in bluetoothdevices: + if name == BLUETOOTH_NAME: + address = addr + print("Found Wiiboard at address " + address) + #if address == None: + #print "No Wiiboards discovered." + return address + + def createBoardEvent(self, data): + buttonBytes = data[0:2] + data = data[2:12] + buttonPressed = False + buttonReleased = False + + state = (buttonBytes[0] << 8) | buttonBytes[1] + if state == BUTTON_DOWN_MASK: + buttonPressed = True + if not self.buttonDown: + self.buttonDown = True + + if buttonPressed == False: + if self.lastEvent.buttonPressed == True: + buttonReleased = True + self.buttonDown = False + + rawTR = (data[0] << 8) + data[1] + rawBR = (data[2] << 8) + data[3] + rawTL = (data[4] << 8) + data[5] + rawBL = (data[6] << 8) + data[7] + + topLeft = self.calcMass(rawTL, TOP_LEFT) + topRight = self.calcMass(rawTR, TOP_RIGHT) + bottomLeft = self.calcMass(rawBL, BOTTOM_LEFT) + bottomRight = self.calcMass(rawBR, BOTTOM_RIGHT) + boardEvent = BoardEvent(topLeft,topRight,bottomLeft,bottomRight,buttonPressed,buttonReleased) + return boardEvent + + + def calcMass(self, raw, pos): + val = 0.0 + #calibration[0] is calibration values for 0kg + #calibration[1] is calibration values for 17kg + #calibration[2] is calibration values for 34kg + if raw < self.calibration[0][pos]: + return val + elif raw < self.calibration[1][pos]: + val = 17 * ((raw - self.calibration[0][pos]) / float((self.calibration[1][pos] - self.calibration[0][pos]))) + elif raw > self.calibration[1][pos]: + val = 17 + 17 * ((raw - self.calibration[1][pos]) / float((self.calibration[2][pos] - self.calibration[1][pos]))) + + return val + + def getEvent(self): + return self.lastEvent + + def getLED(self): + return self.LED + + # Thread that listens for incoming data + def receivethread(self): + while self.status == "Connected": + if True: + data = self.datasocket.recv(25) + intype = data[1] + if intype == INPUT_STATUS: + self.setReportingType() + elif intype == INPUT_READ_DATA: + if self.calibrationRequested == True: + packetLength = data[4]//16 + 1 + self.parseCalibrationResponse(data[7:(7+packetLength)]) + + if packetLength < 16: + self.calibrationRequested = False + + elif intype == EXTENSION_8BYTES: + self.lastEvent = self.createBoardEvent(data[2:12]) + self.mass = self.lastEvent + + else: + print("ACK to data write received") + + self.status = "Disconnected" + self.disconnect() + + def parseCalibrationResponse(self, bytes): + index = 0 + if len(bytes) == 16: + for i in range(2): + for j in range(4): + self.calibration[i][j] = (bytes[index] << 8) + bytes[index+1] + index += 2 + elif len(bytes) < 16: + for i in range(4): + self.calibration[2][i] = (bytes[index] << 8) + bytes[index+1] + index += 2 + + # Send to the Wiiboard + # should be an array of strings, each string representing a single hex byte + def send(self, data): + if self.status != "Connected": + return + senddata = b'0xa2'+data + + self.datasocket.send(senddata) + + def setLight(self, light): + val = 0x00 + if light == True: + val = 0x10 + + msg = bytes([COMMAND_LIGHT, val]) + self.send(msg) + self.LED = light + + def calibrate(self): + msg = bytes([COMMAND_READ_REGISTER, 0x04, 0xA4, 0x00, 0x24, 0x00, 0x18]) + self.send_legacy(msg) + self.calibrationRequested = True + + def setReportingType(self): + msg = bytes([COMMAND_REPORTING, CONTINUOUS_REPORTING, EXTENSION_8BYTES]) + self.send_legacy(bytearr) + +