diff --git a/wiipy_backend.py b/wiipy_backend.py new file mode 100755 --- /dev/null +++ b/wiipy_backend.py @@ -0,0 +1,713 @@ +#!/usr/bin/env python +""" +(c) 2009 Andreas Boehler, andreas.boehler@fh-linz.at, http://klasseonline.dyndns.org +(c) 2009 Patrick Huebner, patrick.huebner@fh-linz.at +based on wiimote.py found online (through Google, doesn't seem to be published anymore) +Source: http://homepage.mac.com/wgwoods/wii/wiimote.py saved as wiimote_orig.py + +v1.9.2, 2009/05/22 +Feature: Speed improvement by 100% by switching from simpleosc to pyliblo! + Drawback: pyliblo must be manually compiled on OS X + +v1.9.1, 2009/05/18 +Fix duplicate packet handling on OS X (again) + +v1.9.0, 2009/05/18 - first version towards a 2.0.0 release +Completely reworked packet handling +Memory reading and writing should be more robust now +Reworked Initialization sequence +BalanceBoard initialization on OS X should work now +Fixed OS X compatibility by handling duplicate packets differently + +v1.8.2, 2009/05/12 +Feature: Nunchuk disconnection works properly + +v1.8.1, 2009/05/12 +Bugfix: BalanceBoard works now (IR must not be enabled on the BB; Setting the +registers seems to influence the BalanceBoard's sensitivity. +Note: On OS X the BalanceBoard sometimes does not work because of limitations +in the Perfect Pairing Patch of OSCulator. Author contacted! + +v1.8, 2009/05/11 +Bugfix: Connection with inserted nunchuk fixed +Bugfix: Restructure +Feature: Removed some cmd-line arguments, thus making backend invocation easier +Feature: Completely reworked initialization sequence + +v1.7.5, 2009/05/11 +Feature: Setup is now completely automatic, no need to configure anymore! +Note: When Nunchuk is connected upon startup, connection fails sometimes! + +v1.7.4, 2009/05/09 +Bugfix: Fix for BalanceBoard Extension Controller +Bugfix: Fix IR not working when Nunchuk connected (initialization error) + +v1.7.3, 2009/05/07 +Bugfix: Device not Found had a logical Error: it could never be reached +Documentation updates everywhere in the code + +v1.7.2, 2009/05/06 +Bugfix: Duplicate Devices found on Win32 are now removed + +v1.7.1, 2009/05/06 +Bugfix: Fixed OSC IR sending code + +v1.7, 2009/05/05 +Feature & Bugfix: Correctly handle duplicate packets (mainly on OS X) + +v1.6.4, 2009/04/27 +Bugfix: Fixed Device Discovery OSC Message on OS X (Unicode String Problem) +Cleaned up the code a bit + +v1.6.3, 2009/04/25 +Bugfix: Correctly exit on OS X +Feature: OSC Message if no device was found + +v1.6.2, 2009/04/25 +Bugfix: Correctly handle OS X connection error + +v1.6.1, 2009/04/24 +Bugfix: Make backend correctly handle CTRL-BREAK + +v1.6, 2009/04/23 +Feature: Added Discovery Routine + +v1.5, 2009/04/21 +Bugfix: Fixed OS X Compatibility + +v1.4, 2009/04/20 (2:00 AM) +Feature: Added support for Nyko Wireless Nunchuk by Sniffing the original handshake +Feature: Added Status support +Feature: Added support for Datel Wirelesse Nunchuk +Feature: Insertion of a Nunchuk is now automatically detected and should be initialized automatically + +v1.3, 2009/04/03 +Feature: Moved existing code into Backend using Command-Line Parameters + +v1.2, 2009/03/31 +Feature: Added basic BalanceBoard support +Bug: Fixed Extension Controller initialization sequence + +v1.1, 2009/03/17 +Feature: Added Nunchuk-Connection Check +Feature: Added some more configuration variable on the top +Feature: Added Nunchuk Buttons -> Nunchuk is now FULLY SUPPORTED! + +v1.0, 2009/03/17 +Feature: Added IR support in Nunchuk Mode +Feature: Added ACC support in Nunchuk Mode +Feature: Added OSC support for IR +Feature: Added OSC support for Nunchuk Accelerometers + +v0.4, 2009/03/16 +Feature: Added (untested) Nunchuk support + +v0.3, 2009/03/13 +Bug: Fixed OS X compatibility (one MUST install lightblue using sudo!) + +v0.2, 2009/03/12 +Feature: Added OSC button support +Feature: Added (untested) Mac support + +v0.1, 2009/03/06 +Feature: Initial OSC functionality (ACC only) +Feature: Runs correctly on XP and Linux + +ToDo: +* Cleanup the Code! +* If no Nunchuk is present, but activated, C/Z are always activated! +""" + +import sys +if sys.platform == "darwin": + import lightblue +else: + import bluetooth +# The following enables correct termination on OS X and Win32 +import signal +if sys.platform == "win32": + signal.signal(signal.SIGBREAK, signal.default_int_handler) +else: + signal.signal(15, signal.default_int_handler) +import math +import time +import liblo +import struct + +# We control the Backend using command-line parameters; one can launch the backend +# directly or use the frontend to control the backend using OSC. +from optparse import OptionParser + +parser = OptionParser() + +parser.add_option("-v","--verbose",dest="verbose", action="store_true", default=False, help="output extra information") +parser.add_option("-a","--address", dest="address", help="WiiMote Address to connect to") +parser.add_option("-n","--number", dest="number", help="Number of Wiimote", default=0, type="int") +parser.add_option("-o","--osc", dest="osc_ip", help="OSC IP Address", default="127.0.0.1") +parser.add_option("-p","--port", dest="osc_port", help="OSC Port", default=5600, type="int") +parser.add_option("-s", "--scan", dest="scan", help="Scan for Devices", default=False, action="store_true") + +(opt,argv) = parser.parse_args() + +if not opt.address: + parser.error("No Address given, will now quit") + +OSC_IP = opt.osc_ip.strip() +OSC_PORT = opt.osc_port + +def i2bs(x): + '''Convert a (32-bit) int to a list of 4 byte values, e.g. + i2bs(0xdeadbeef) = [222,173,190,239] + 12bs(0x4) = [0,0,0,4]''' + out=[] + while x or len(out) < 4: + out = [x & 0xff] + out + x = x >> 8 + return out + +buttonmap = { + '2': 0x0001, + '1': 0x0002, + 'B': 0x0004, + 'A': 0x0008, + '-': 0x0010, + 'H': 0x0080, + 'L': 0x0100, + 'R': 0x0200, + 'D': 0x0400, + 'U': 0x0800, + '+': 0x1000, +} + +nunchmap = { + 'C': 0x0002, + 'Z': 0x0001, +} + +statusmap = { + 'Bat': 0x01, + 'Ext': 0x02, + 'Spk': 0x04, + 'IR' : 0x08, + 'L1' : 0x10, + 'L2' : 0x20, + 'L3' : 0x40, + 'L4' : 0x80, +} + +# BLUH. These should be less C-ish. +CMD_SET_REPORT = 0x52 + +RID_LEDS = 0x11 +RID_MODE = 0x12 +RID_IR_EN = 0x13 +RID_SPK_EN = 0x14 +RID_STATUS = 0x15 +RID_WMEM = 0x16 +RID_RMEM = 0x17 +RID_SPK = 0x18 +RID_SPK_MUTE = 0x19 +RID_IR_EN2 = 0x1a + +MODE_BASIC = 0x30 +MODE_ACC = 0x31 +MODE_IR = 0x33 # 0x32 Shouldn't this be 0x33??? +MODE_FULL = 0x3e # Currently unused +MODE_EXT = 0x37 # We should use this mode to read from the Nunchuk +MODE_BB = 0x32 # This is used for the BalanceBoard + +IR_MODE_OFF = 0 +IR_MODE_STD = 1 +IR_MODE_EXP = 3 +IR_MODE_FULL = 5 + +FEATURE_DISABLE = 0x00 +FEATURE_ENABLE = 0x04 + +# Max value for IR dots +DOT_MAX = 0x3ff + +""" +The Wiimote class handles all communication and is responsible for +receiving and decrypting packets +""" +class Wiimote(object): + def __init__(self,addr,number=0): + self.target = liblo.Address(OSC_IP, OSC_PORT) + if sys.platform == 'darwin': + self.osx = True + else: + self.osx = False + self.connected=False + self.done=False + self.addr=addr + self.number=number + self.mode = 0 + self.ledmask = 0 + self.buttonmask = 0 + self.nunchmask = 0 + self.force = [0,0,0] + self.force_nunch= [0,0,0] + self.stick_nunch= [0,0] + self.force_zero = [0,0,0] + self.bb_force = [0,0,0,0] + self.force_1g = [0,0,0] + self.statusmask = 0 + self.batterylevel = 0 + self.ack = False + self.memresult = False + self.bundle = None + self.force_1g_diff = [0,0,0] # Difference between zero and 1g + self.dots = [DOT_MAX,DOT_MAX,DOT_MAX,DOT_MAX,DOT_MAX,DOT_MAX,DOT_MAX,DOT_MAX] + + if self.osx: + self.rx = lightblue.socket(lightblue.L2CAP) + self.cx = lightblue.socket(lightblue.L2CAP) + else: + self.rx = bluetooth.BluetoothSocket(bluetooth.L2CAP) + self.cx = bluetooth.BluetoothSocket(bluetooth.L2CAP) + + def connect(self): + try: + self.rx.connect((self.addr,19)) + self.cx.connect((self.addr,17)) + self.connected = True + except: + self.connected = False + print "Connection Failed" + self.initialize() + + def disconnect(self): + self.cx.close() + self.rx.close() + self.connected=False + + def mainloop(self): + while not self.done: + self._getpacket() + + def initialize(self): + self.enable_extension() + self.getstatus() + while self.batterylevel == 0: # batterylevel is used b/c self.statusmask=0 if no extension is inserted + self._getpacket() + if self.mode != MODE_BB: + self.setled(self.number) + + def _handle_button_data(self,data): # Handle Button Data + buttonlist='+UDLRH-AB12' + newmask = (ord(data[2])<<8)+ord(data[3]) + self.buttonmask = newmask + for c in buttonlist: + if self.buttonmask & buttonmap[c]: + self.bundle.add(liblo.Message("/wii/"+str(self.number)+"/button/"+c, 1)) + + def _handle_status_data(self, data): # Status Data handling + if len(data) != 8: return False + statuslist = ['Bat', 'Ext', 'Spk', 'IR', 'L1', 'L2', 'L3', 'L4'] + newmask = ord(data[4]) + if newmask & statusmap['Ext'] and not self.statusmask & statusmap['Ext']: + print "Extension inserted" + self.enable_extension() # We repeat the initalization sequence; this is a workaround for the Datel Wireless Nunchuk + self.enable_extension() + if not newmask & statusmap['Ext'] and self.statusmask & statusmap['Ext']: + print "Extension removed" + self.enable_extension() + self.statusmask = newmask + self.batterylevel = ord(data[7]) + for c in statuslist: + if self.statusmask & statusmap[c]: + self.bundle.add(liblo.Message("/wii/"+str(self.number)+"/status/"+c, 1)) + else: + self.bundle.add(liblo.Message("/wii/"+str(self.number)+"/status/"+c, 0)) + print self.batterylevel + self.bundle.add(liblo.Message("/wii/"+str(self.number)+"/batterylevel", self.batterylevel)) + + def _handle_force_data(self,data): + if len(data) != 3: return False + self.force = [ord(d) for d in data] + self.bundle.add(liblo.Message("/wii/"+str(self.number)+"/acc", self.force[0], self.force[1], self.force[2])) +# self.force_calib = [self.force[0] - self.force_zero[0], self.force[1] - self.force_zero[1], self.force[2] - self.force_zero[2]] + return True + + def _handle_bb_data(self, data): # Handle BalanceBoard Data + a,b,c,d,e,f,g,h = [ord(d) for d in data] + tr = (a << 8) + b + br = (c << 8) + d + tl = (e << 8) + f + bl = (g << 8) + h + self.bb_force = [tr,br,tl,bl] + self.bundle.add(liblo.Message("/wii/"+str(self.number)+"/bb/force", self.bb_force[0], self.bb_force[1], self.bb_force[2])) + + def _handle_ext_data(self,data): # This gives only the raw Extension data + nunchlist = 'CZ' + acc = data[2:5] + stick = data[0:2] + self.nunchmask = ord(data[5]) & 0x03 + self.force_nunch = [ord(d) for d in acc] + self.stick_nunch = [ord(d) for d in stick] + for c in nunchlist: + if not self.nunchmask & nunchmap[c]: + self.bundle.add(liblo.Message("/wii/"+str(self.number)+"/nunchuk/button/"+c, 1)) + self.bundle.add(liblo.Message("/wii/"+str(self.number)+"/nunchuk/acc", self.force_nunch[0], self.force_nunch[1], self.force_nunch[2])) + self.bundle.add(liblo.Message("/wii/"+str(self.number)+"/nunchuk/stick", self.stick_nunch[0], self.stick_nunch[1])) + return True + + def _handle_IR_data(self,data): # Handle Infrared Data + """ + There are two (in fact, three) possible infrared modes we need to distinguish + basic, extended and full, but only basic and extended are covered here + """ + if len(data) == 12: # The Mote can track 4 points + self.dots = self._handle_IR_ext(data[0:6]) + self.dots.extend(self._handle_IR_ext(data[6:12])) + elif len(data) == 10: + self.dots = self._handle_IR_basic(data[0:5]) + self.dots.extend(self._handle_IR_basic(data[5:10])) + else: return False + self.bundle.add(liblo.Message("/wii/"+str(self.number)+"/irdata", self.dots[0], self.dots[1], self.dots[2], self.dots[3], self.dots[4], self.dots[5], self.dots[6], self.dots[7])) + + def _handle_IR_basic(self,data): # Handle Basic IR Data + if data == '\xff'*5: # No Dot found + dots = [DOT_MAX,DOT_MAX,DOT_MAX,DOT_MAX] + else: + a,b,c,d,e = [ord(d) for d in data] + x1 = a+((c & 0x30) << 4) + y1 = b+((c & 0xc0) << 2) + x2 = d+((c & 0x03) << 8) + y2 = e+((c & 0x0c) << 6) + dots = [x1,y1,x2,y2] + return dots + + def _handle_IR_ext(self,data): # Handle extended IR data + if len(data) != 6: return False + if data ==' \xff'*6: + dots=[DOT_MAX,DOT_MAX,DOT_MAX,DOT_MAX] + else: + a,b,c,d,e,f = [ord(d) for d in data] + # processing dots: + # each tuple is 3 bytes in the form: x,y,extra + # extra contains 8 bits of extra data as follows: [yyxxssss] + # x and y are the high two bits for the full 10-bit x/y values. + # s is size data + x1=a+((c & 0x30) << 4) + y1=b+((c & 0xc0) << 2) + x2=d+((f & 0x30) << 4) + y2=e+((f & 0xc0) << 2) + dots=[x1,y1,x2,y2] + return dots + + def _getpacket(self): # Receive packet and call handling routine + try: + data = self.rx.recv(1024) + self._handlepacket(data) + except: + print "Error receiving data" + self.done = True + + def _handlepacket(self, data): # This handles packet data + """ Duplicate packets are handled recursively: The packet length + for the returned mode is checked against the appropriate length. + If that doesn't match, the remaining string is handled again. + If the remaining string is too short, Data is received again. + """ + self.bundle = liblo.Bundle() + if len(data) < 2: + data += self.rx.recv(1024) + if data.startswith('\xa1\x20'): # status + if len(data) < 8: + data += self.rx.recv(1024) + self._handle_status_data(data[0:8]) + self._handle_button_data(data[0:4]) + liblo.send(self.target, self.bundle) + if len(data) > 8: + self._handlepacket(data[8:]) + + elif data.startswith('\xa1\x21'): # Read Memory + if len(data) < 23: + data += self.rx.recv(1024) + self.memresult = data[0:23] + if len(data) > 23: + self._handlepacket(data[23:]) + + elif data.startswith('\xa1\x22'): # Acknowledge Command + if len(data) < 6: + self.rx.recv(1024) + self.ack = True + self._handle_button_data(data[0:4]) + liblo.send(self.target, self.bundle) + if len(data) > 6: + self._handlepacket(data[6:]) + + elif data.startswith('\xa1\x30'): # button + if len(data) < 4: + data += self.rx.recv(1024) + self._handle_button_data(data[0:4]) + liblo.send(self.target, self.bundle) + if len(data) > 4: + self._handlepacket(data[4:]) + + elif data.startswith('\xa1\x31'): # button + accelerometer + if len(data) < 7: + data += self.rx.recv(1024) + self._handle_button_data(data[0:4]) + self._handle_force_data(data[4:7]) + liblo.send(self.target, self.bundle) + if len(data) > 7: + self._handlepacket(data[7:]) + + elif data.startswith('\xa1\x32'): # button + 8 extensions bytes (BalanceBoard) + if len(data) < 12: + data += self.rx.recv(1024) + self._handle_button_data(data[0:4]) + self._handle_bb_data(data[4:12]) + liblo.send(self.target, self.bundle) + if len(data) > 12: + self._handlepacket(data[12:]) + + elif data.startswith('\xa1\x33'): # button + accel + IR + if len(data) < 19: + data += self.rx.recv(1024) + self._handle_button_data(data[0:4]) + self._handle_force_data(data[4:7]) + self._handle_IR_data(data[7:19]) + liblo.send(self.target, self.bundle) + if len(data) > 19: + self._handlepacket(data[19:]) + + elif data.startswith('\xa1\x37'): # button + accel + IR + Nunchuk + if len(data) < 23: + data += self.rx.recv(1024) + self._handle_button_data(data[0:4]) + self._handle_force_data(data[4:7]) + self._handle_IR_data(data[7:17]) + self._handle_ext_data(data[17:23]) + liblo.send(self.target, self.bundle) + if len(data) > 23: + self._handlepacket(data[23:]) + + elif len(data) == 0: # Wiimote went away! + self.done = True + else: + print "Unknown packet len %i: 0x%s" % (len(data),data.encode("hex")) + + def setled(self,num): # Enable a given LED + if num < 4: + self.ledmask = self.ledmask | (0x10 << num) + self._led_command() + + def clearled(self,num): # Disable a given LED + if num < 4: + self.ledmask = self.ledmask & ~(0x10 << num) + self._led_command() + + def getstatus(self): # Read the WiiMotes Status + self._send_command(CMD_SET_REPORT,RID_STATUS,[0]) + + def buttons_str(self): # Used only for Verbose Output + buttonlist='+UDLRH-AB12' + nunchlist = 'CZ' + out='' + for c in buttonlist: + if not self.buttonmask & buttonmap[c]: + c = '.' + out = out + c + for c in nunchlist: + if self.nunchmask & nunchmap[c]: + c = '.' + out = out + c + return out + + def force_str(self): # Used only for Verbose Output + return "(% 4i,% 4i,% 4i)" % (self.force[0]-self.force_zero[0], + self.force[1]-self.force_zero[1], + self.force[2]-self.force_zero[2]) + + def dots_str(self): # Used only for Verbose Output + #print self.dots + return "((%4i,%4i),(%4i,%4i))" % (self.dots[0], self.dots[1], self.dots[2], self.dots[3]) + + def status_str(self): # Used only for Verbose Output + return "%i: %s force=%s dots=%s stick=%s nf=%s bb=%s" % \ + (self.number,self.buttons_str(),self.force_str(),self.dots_str(),str(self.stick_nunch), str(self.force_nunch), str(self.bb_force)) + + def showstatus(self): # Verbose Output + sys.stdout.write(self.status_str() + "\r") + sys.stdout.flush() + + def setmode(self,mode): # Send desired Mode to Mote + self.mode = mode + # XXX wiimotulator.py has flags for setting 0x01 in the first byte for + # 'rmbl' and 0x04 for 'cont'. Both of these are always off. + # No idea why. + self._send_command(CMD_SET_REPORT,RID_MODE,[0,mode]) + + def enable_force(self): # Enable Accelerometer and read Calibration Data + #self.setmode(self.mode | MODE_ACC) + self.get_force_calibration() + + def enable_IR(self): # Enable the Infrared Device + if self.mode != MODE_EXT and self.mode != MODE_BB: # TODO: MODE_BB is not used here, right? + self.setmode(self.mode | MODE_IR) + self._send_command(CMD_SET_REPORT,RID_IR_EN,[FEATURE_ENABLE]) + self._send_command(CMD_SET_REPORT,RID_IR_EN2,[FEATURE_ENABLE]) + # Enable IR device + self._write_mem(0x04b00030,[0x01]) + # Set sensitivity constants + self._write_mem(0x04b00030,[0x08]) + self._write_mem(0x04b00006,[0x90]) # Write Block1 in 2 individual bytes; Suggested by Marcan + self._write_mem(0x04b00008,[0xc0]) # Write Block1 second half; Suggested by Marcan + self._write_mem(0x04b0001a,[0x40]) # Write Block2 as suggested by Marcan + if self.mode != 0x37: # This is Extended Mode + self._write_mem(0x04b00033,[0x33]) # Write Mode Number; Why is it 0x33?? + else: + self._write_mem(0x04b00033,[0x31]) # Basic Mode + # Enable IR data output + self._write_mem(0x04b00030,[8]) + + def enable_extension(self): # Enable Nunchuk; This procedure should work on Original and 3rd party Nunchuks! + self._write_mem(0x04a400f0,[0x55]) + self._write_mem(0x04a400fb,[0x00]) + ext_data = self._read_mem(0x04a400fa,6) + if ext_data[2:6] == "\xa4\x20\x00\x00": + self._write_mem(0x04a40040,[0x05, 0xa0, 0xb2, 0x1d, 0x98, 0xac]) # Set up 16 byte encryption key + self._write_mem(0x04a40046,[0x8b, 0x26, 0xc1, 0xd9, 0x39, 0x64]) # It is necessary for 3rd party Nunchuk + self._write_mem(0x04a4004c,[0x52, 0x0c, 0x73, 0x05]) + self.setmode(MODE_EXT) # The mode MUST be set right now, not beforehand (3rd party Nunchuk!) + print "Nunchuk connected, using 10 Byte IR" + self.enable_IR() # IR needs to be re-enabled to work correctly + elif ext_data[2:6] == "\xa4\x20\x04\x02": + self.setmode(MODE_BB) + self.setled(0) + print "BalanceBoard connected" + self.get_bb_calibration() + else: + print "Extension not connected, using 12 Byte IR" + self.setmode(MODE_IR) + self.enable_force() + self.enable_IR() + + def get_bb_calibration(self): # This is not a very nice function... + bundle = liblo.Bundle() + data1 = self._read_mem(0x04a40024, 12) # Get the first half of calibration data + data2 = self._read_mem(0x04a40030, 12) + if data1: + a,b,c,d,e,f,g,h,i,j,k,l,m,n,o,p = [ord(d) for d in data1] + tr0 = (a << 8) + b + br0 = (c << 8) + d + tl0 = (e << 8) + f + bl0 = (g << 8) + h + tr17 = (i << 8) + j + br17 = (k << 8) + l + if data2: + a,b,c,d,e,f,g,h,i,j,k,l,m,n,o,p = [ord(d) for d in data2] + tl17 = (a << 8) + b + bl17 = (c << 8) + d + tr34 = (e << 8) + f + br34 = (g << 8) + h + tl34 = (i << 8) + j + bl34 = (k << 8) + l + if data1 and data2: + self.bb_calib = [tr0,br0,tl0,bl0,tr17,br17,tl17,bl17,tr34,br34,tl34,bl34] + #print self.bb_calib + bundle.add(liblo.Message("/wii/"+str(self.number)+"/bb/calib", tr0, br0, tl0, bl0, tr17, br17, tl17, bl17, tr34, br34, tl34, bl34)) + liblo.send(self.target, bundle) + + def get_force_calibration(self): # Retrieve Acc calibration Data + # FIXME: calib data is never sent via OSC + data = None + n = 0 + while data == None and n<32: + data = self._read_mem(0x16,10) + n = n+1 + if data: + data=[ord(b) for b in data] + self.force_zero = data[0:3] + self.force_1g = data[4:7] + # XXX currently we don't know what data[3], data[7], or data[8:9] are + # Calculate the difference between zero and 1g for each axis + for b in range(0,3): + self.force_1g_diff[b] = self.force_1g[b] - self.force_zero[b] + + def _led_command(self): # Send LED commands to the WiiMote + self._send_command(CMD_SET_REPORT,RID_LEDS,[self.ledmask]) + + def _waitforok(self): # Wait for OK/ACK packet + n = 0 + while not self.ack and n<32: + self._getpacket() + n = n + 1 + self.ack = False + + def _read_mem(self,offset,size): # Read a given memory address + if size >= 16: + print "ERROR: _read_mem can't handle size > 15 yet" + return None + # RMEM command wants: [offset,size] + self._send_command(CMD_SET_REPORT,RID_RMEM,i2bs(offset)+[0,size]) + n = 0 + while not self.memresult and n<32: + self._getpacket() + n = n + 1 + data = self.memresult + self.memresult = False + if data: + # TODO check error flag, continuation, etc + return data[7:] + else: + return None + + def _write_mem(self,offset,data): # Write to memory + # WMEM command wants: [offset,size,data] + # offset = 32-bit, bigendian. data is 0-padded to 16 bytes. + size = len(data) + if size > 16: return False # Too much data! + if size < 16: data = data + [0]*(16-size) + self._send_command(CMD_SET_REPORT,RID_WMEM,i2bs(offset)+[size]+data) + self._waitforok() + + def _send_command(self,cmd,report,data): # Send a specific command + self.cx.send(chr(cmd) + chr(report) + "".join([chr(d) for d in data])) + +if __name__ == '__main__': + if opt.scan: + target = liblo.Address(OSC_IP, OSC_PORT) + print 'Discovering Bluetooth Devices, please wait...' + if sys.platform == 'darwin': + devicelist = lightblue.finddevices() + else: + devicelist = bluetooth.discover_devices(lookup_names = True) + + devicefound = False + if len(devicelist) > 0: # If devices are found, cycle through the list and check the names + devicelist = list(set(devicelist)) # This removes duplicates on Win32; really nice function! + for ii in range(0,len(devicelist)): + if devicelist[ii][1] == 'Nintendo RVL-CNT-01' or devicelist[ii][1] == 'Nintendo RVL-WBC-01': + liblo.send(target, liblo.Message('/wii/devices', devicelist[ii][0], devicelist[ii][1].encode())) + print 'Found: ' + devicelist[ii][0] + ' ' + devicelist[ii][1] + devicefound = True + + if devicefound: + print "Done discovering" + print "Continue with connecting" + devicefound = False + else: + liblo.send(target, liblo.Message('/wii/devices', '0', 'nodev')) + print "No device was found. - Try again" + else: # No scan was requested, just start to capture packets + print "Connecting - press 1+2 on your Wiimote." + + w = Wiimote(opt.address.strip(), opt.number) + w.connect() + if w.connected: # Only enable and initalize the controller, if connection was successful +# w.getstatus() # Before we start capturing, we retrieve status information + + try: + last=time.time() + while not w.done: + w._getpacket() + if opt.verbose: + w.showstatus() + finally: + w.disconnect() + diff --git a/wiipy_frontend.py b/wiipy_frontend.py new file mode 100755 --- /dev/null +++ b/wiipy_frontend.py @@ -0,0 +1,247 @@ +#!/usr/bin/env python +""" +Frontend for wiipy_backend.py +(c) 2009 Andreas Boehler, andreas.boehler@fh-linz.at, http://klasseonline.dyndns.org +(c) 2009 Patrick Huebner, patrick.huebner@fh-linz.at + +Requirement: + * pybluez (Windows/Linux), lightblue (OS X) + * pyliblo (all platforms) + +Notes: + * wiimote_backend.py and wiimote_frontend.py need to be in the same dir! + * The current dir must be set to wiimote_[frontend|backend].py + * The Microsoft-BT Stack is NOT supported! Tested: Widcomm under XP + * The path in OS X must not contain spaces! + +v1.8, 2009/05/22 +Feature: Migrated to pyliblo from simpleosc which is a major speed improvement + +v1.7, 2009/05/11 +Feature: Removed setup sequence, initialization is now completely automatic; only left: debug + +v1.6.3, 2009/05/08 +Bugfix: Forgot int(msg) in disconnect-code + +v1.6.2, 2009/05/06 +Bugfix: Path to python.exe is now taken from sys.executable (should work on all platforms) + +v1,6.1, 2009/05/05 +Feature: Added debugging/verbose parameter support + +v1.6, 2009/04/28 +Feature: All OSC IPs and Ports can be freely configured now! + +v1.5.1, 2009/04/27 +Feature: Added ping command, status still to be done + +v1.5, 2009/04/26 +Bugfix: Correctly Terminate the process on MacOS X. + +v1.4, 2009/04/24 +Bugfix: Correctly Terminate the process on Win32. This took me about 6h to figure out! + +v1.3.1, 2009/04/23 +Bugfix: Changed from SIGKILL to SIGTERM for Unix kill method + +v1.3, 2009/04/23 +Feature: Added Device Discovery Routine +Bugfix: Fixed Python 2.6 compatibility + +v1.2, 2009/04/23 +Bugfix: Frontend used 100% CPU because of infinite loop + +v1.1, 2009/04/20 +Bugfix: Fixed OS X Compatibility + +v1.0, 2009/04/09 +Bugfix: Fixed Windows Compatibility (tested on Vista) +Note: Path to python.exe is hardcoded for now! + +v0.2, 2009/04/08 +Feature: Added OSC support + +v0.1, 2009/04/07 +Feature: Initial Version w/o OSC support + +OSC Format description: +Address: "/wiipy" +Format: [Type, Number, Options] + +Tuple: ["setup", 0, "debug", 1] +Tuple: ["connect", 0, "00:4f:4e:13:56:40"] +Tuple: ["disconnect", 0] +Tuple: ["shutdown"] stops alls connections and quits the wiipy_frontend.py +Tuple: ["discover"] starts Bluetooth-Discovery and sends the results via OSC +Tuple: ["ping"] sends a simple "OK" via OSC +Tuple: ["status"] reports connections via OSC +Tuple: ["config", "ip", "127.0.0.1", "port", 5600] configure osc IP and Port to connect to +""" + +import os +import subprocess +import sys +import time as timer +from liblo import * +from optparse import OptionParser + +parser = OptionParser() + +parser.add_option("-i","--ip", dest="ip", help="OSC Server Address", default="127.0.0.1") +parser.add_option("-p","--port", dest="port", help="OSC Server Port", default=5601, type="int") +parser.add_option("-d", "--debug", dest="debug", help="Enable debugging", default=False, action="store_true") + +(opt,argv) = parser.parse_args() + +OSC_IP = opt.ip.strip() +OSC_PORT = opt.port + +class spawn_wiimote(object): + """ +This class spawns another process using various cmd-line arguments. +It is used to run the backend with the necessary arguments, because +threads are not safe using lightblue + """ + + def __init__(self, filename, args): + self.filename = filename + self.args = args + + def spawn(self): + # We use the new subprocess class to spawn the command + if sys.platform == "win32": + self.process = subprocess.Popen([self.filename] + self.args, creationflags=512) + else: + self.process = subprocess.Popen([self.filename] + self.args) + self.pid = self.process.pid + if opt.debug: + print self.args + print "Spawned wiipy_backend.py with PID: " + str(self.pid) + + def kill(self): + # The kill method depends on the platform, the win32 code took me forever to figure out! + if not self.process.poll(): + if opt.debug: + print "Killing Process..." + if sys.platform == 'win32': + import ctypes + ctypes.windll.kernel32.GenerateConsoleCtrlEvent(1, self.pid) + else: # This is the Unix Method of killing a PID + os.kill(self.pid, 15) + + def status(self): + if not self.process.poll(): + return True + else: + return False + +class oscServer(ServerThread): + """ +This class defines an OSC Server Process, parses the options sent and +spawns the backend + """ + def __init__(self, ip='127.0.0.1', port=5601): + self.ip = ip + self.port = port + ServerThread.__init__(self, self.port) + self.osc_ip = '127.0.0.1' + self.osc_port = 5600 + self.debug = [0, 0, 0, 0] + self.address = [0, 0, 0, 0] + self.wiimote = [None, None, None, None] + self.connected = True + if sys.platform == 'darwin': + self.osx = True + else: + self.osx = False + print "Listening on " + self.ip + ':' + str(self.port) + + @make_method('/wiipy', None) + def wiipy_callback(self, path, args): #Yeah, this is ugly, I know... + if args[0] == "setup": + print "Setting up: " + str(int(args[1])) + nr = int(args[1]) + if args[2] == "debug": + self.debug[nr] = int(args[3]) + if opt.debug: + print self.debug + elif args[0] == "connect": + address = args[2] + nr = int(args[1]) + print "Connecting: " + str(nr) + " to " + address + args = ["-a " + address, "-n " + str(nr), "-o " + self.osc_ip, "-p " + str(self.osc_port)] + if opt.debug: + print args + if self.debug[nr] == 1: + args.append("-v") + if self.wiimote[nr] == None: + args.insert(0, 'wiipy_backend.py') + self.wiimote[nr] = spawn_wiimote(sys.executable, args) + self.wiimote[nr].spawn() + elif args[0] == "disconnect": + nr = int(args[1]) + print "Disconnecting from: " + str(nr) + if self.wiimote[nr] != None: + self.wiimote[nr].kill() + self.wiimote[nr] = None + elif args[0] == "shutdown": + self.stopOsc() + elif args[0] == "discover": + self.discover_devices() + elif args[0] == "ping": + if opt.debug: + print "Ping successful" + send(Address(self.osc_ip, self.osc_port), Message('/wii/ping', 'OK')) + elif args[0] == "status": + self.get_status() + elif args[0] == "config": + for ii in range(0,len(args)-1): + if args[ii+1] == "ip": + self.osc_ip = args[ii+2] + elif args[ii+1] == "port": + self.osc_port = int(args[ii+2]) + print 'Now sending to: ' + self.osc_ip + ':' + str(self.osc_port) + + def stopOsc(self): + print "Disconnecting all Wiimotes" + for ii in range(0,4): + if self.wiimote[ii] != None: + self.wiimote[ii].kill() + print "Stop OSC" + self.connected = False + + def get_status(self): # Get the current Status of connected WiiMotes + print "Status" + statslist = [0, 0, 0, 0] + for ii in range(0,4): + if self.wiimote[ii] != None: + if self.wiimote[ii].status: + statslist[ii] = 1 + if opt.debug: + print statslist + send(Address(self.osc_ip, self.osc_port), Message('/wii/status', statslist[0], statslist[1], statslist[2], statslist[3])) + + def discover_devices(self): #The discoverer is in reality the backend, so we spawn it + args = ["wiipy_backend.py", "-a 00:00:00:00:00:00", "-s", "-o " + self.osc_ip, "-p " + str(self.osc_port)] + if opt.debug: + print args + sp = spawn_wiimote(sys.executable, args) + sp.spawn() + +if __name__ == '__main__': + """ +We only need a short OSC Server and need to handle termination correctly, +that is by stopping the server and disconnecting all clients. +Note that the OSC Server is, at least under Linux, only stopped upon reception +of the next message! + """ + + s = oscServer(OSC_IP, OSC_PORT) + s.start() + print "wiipy_frontend.py initialized, waiting for commands" + try: + while s.connected: + timer.sleep(1) + except: + print "Exit" + s.stopOsc()