forked from Blinkenbunt/blup
klonfish
2 changed files with 442 additions and 0 deletions
@ -0,0 +1,201 @@
@@ -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() |
||||
|
||||
@ -0,0 +1,241 @@
@@ -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 <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 <data> to the Wiiboard |
||||
# <data> 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) |
||||
|
||||
|
||||
Loading…
Reference in new issue