From e56e03080649a09f0678caf763df2c32a5accbbf Mon Sep 17 00:00:00 2001 From: wiese4 Date: Tue, 6 Oct 2015 17:08:18 +0200 Subject: [PATCH 1/3] Added Gyroscope class --- wiimote.py | 92 ++++++++++++++++++++++++++++++++++++++++++++++++++++++ 1 file changed, 92 insertions(+) diff --git a/wiimote.py b/wiimote.py index 219a32a..491e560 100644 --- a/wiimote.py +++ b/wiimote.py @@ -119,6 +119,94 @@ def _debug(msg): print("DEBUG: " + str(msg)) +class Gyroscope(object): + """ + Represents the gyroscope of the Wiimote. + """ + + SUPPORTED_REPORTS = [0x32, 0x37] + + def __init__(self, wiimote): + self._state = [0.0, 0.0, 0.0] + self.wiimote = wiimote + self._com = wiimote._com + self._callbacks = [] + + def __len__(self): + return len(self._state) + + def __repr__(self): + return repr(self._state) + + def __getitem__(self, axis): + if 0 <= axis <= 2: + return self._state[axis] + else: + raise IndexError("list index %d out of range" % (axis)) + + def register_callback(self, func): + """ + Register a callback function `func` that gets called every time + when new gyroscope values are transmitted from the Wiimote. + A list with XYZ gyroscope values is passed + to the callback function. + """ + self._callbacks.append(func) + + def unregister_callback(self, func): + """ + Unregister a callback function `func` that has been previously registered. + The function will no longer get called on new gyroscope data from the Wiimote. + """ + if func in self._callbacks: + self._callbacks.remove(func) + + def _notify_callbacks(self): + for callback in self._callbacks: + callback(self._state) + + def handle_report(self, report): + """ + Extract gyroscope data from a Wiimote report. + Usually gets called by the Wiimote CommunicationHandler object. + """ + if self.report_type == self.SUPPORTED_REPORTS[0]: + z_msb, y_msb, x_msb, Z_msb, Y_msb, X_msb = report[3:9] + elif self.report_type == self.SUPPORTED_REPORTS[1]: + z_msb, y_msb, x_msb, Z_msb, Y_msb, X_msb = report[16:22] + + x = ((X_msb & 0xFC) << 6) | x_msb + y = ((Y_msb & 0xFC) << 6) | y_msb + z = ((Z_msb & 0xFC) << 6) | z_msb + + self._state = [x, y, z] + self._notify_callbacks() + + def activate(self, rep_mode=0x37): + """ + Activates the extension controller and sets the report mode to get data from the gyroscope. + Gets called by the activate method of the Wiimote class. + """ + data = 0x04 + reg_addr = 0xA600FE + self.report_type = rep_mode + self.wiimote.memory.write(reg_addr, data) + self._com.set_report_mode(rep_mode) + + def deactivate(self): + """ + Deactivates the extension controller and sets the default report mode. + Gets called by the activate method of the Wiimote class. + """ + data = 0x55 + reg_addr = 0xA400F0 + self.wiimote.memory.write(reg_addr, data) + data = 0x00 + reg_addr = 0xA400FB + self.wiimote.memory.write(reg_addr, data) + self._com.set_report_mode(0x33) + + class Accelerometer(object): """ Represents the accelerometer of the Wiimote. @@ -502,6 +590,7 @@ class CommunicationHandler(threading.Thread): MODE_DEFAULT = 0x30 MODE_ACC = 0x31 + MODE_EXTENSION = 0x32 MODE_ACC_IR = 0x33 RPT_STATUS_REQ = 0x15 @@ -575,6 +664,8 @@ def _handle(self, bytes_read): self.wiimote.memory.handle_report(bytes_read[1:]) if rpt_type in IRCam.SUPPORTED_REPORTS: self.wiimote.ir.handle_report(bytes_read[1:]) + if rpt_type in Gyroscope.SUPPORTED_REPORTS: + self.wiimote.gyroscope.handle_report(bytes_read[1:]) def set_rumble(self, state): self.rumble = state @@ -592,6 +683,7 @@ def __init__(self, btaddr, model): self._com = CommunicationHandler(self) self._leds = LEDs(self) self.accelerometer = Accelerometer(self) + self.gyroscope = Gyroscope(self) self.buttons = Buttons(self) self.rumbler = Rumbler(self) self.memory = Memory(self) From fcb6aea607aa96462fed7d50e64b6fa6dfc08ec4 Mon Sep 17 00:00:00 2001 From: wiese4 Date: Tue, 6 Oct 2015 17:35:32 +0200 Subject: [PATCH 2/3] Refactored Gyroscope class --- wiimote.py | 40 +++++++++++++++++++++------------------- 1 file changed, 21 insertions(+), 19 deletions(-) diff --git a/wiimote.py b/wiimote.py index 491e560..e610aac 100644 --- a/wiimote.py +++ b/wiimote.py @@ -151,6 +151,7 @@ def register_callback(self, func): A list with XYZ gyroscope values is passed to the callback function. """ + self._callbacks.append(func) def unregister_callback(self, func): @@ -158,6 +159,7 @@ def unregister_callback(self, func): Unregister a callback function `func` that has been previously registered. The function will no longer get called on new gyroscope data from the Wiimote. """ + if func in self._callbacks: self._callbacks.remove(func) @@ -170,14 +172,18 @@ def handle_report(self, report): Extract gyroscope data from a Wiimote report. Usually gets called by the Wiimote CommunicationHandler object. """ - if self.report_type == self.SUPPORTED_REPORTS[0]: - z_msb, y_msb, x_msb, Z_msb, Y_msb, X_msb = report[3:9] - elif self.report_type == self.SUPPORTED_REPORTS[1]: - z_msb, y_msb, x_msb, Z_msb, Y_msb, X_msb = report[16:22] - x = ((X_msb & 0xFC) << 6) | x_msb - y = ((Y_msb & 0xFC) << 6) | y_msb - z = ((Z_msb & 0xFC) << 6) | z_msb + # How to read the reports from: + # http://wiibrew.org/wiki/Wiimote and http://abstrakraft.org/cwiid/wiki/MotionPlus + if self.report_type == 0x32: + zz, yy, xx, ZZ, YY, XX = report[3:9] + elif self.report_type == 0x37: + zz, yy, xx, ZZ, YY, XX = report[16:22] + + # Calculation from http://abstrakraft.org/cwiid/wiki/MotionPlus + x = ((XX & 0xFC) << 6) | xx + y = ((YY & 0xFC) << 6) | yy + z = ((ZZ & 0xFC) << 6) | zz self._state = [x, y, z] self._notify_callbacks() @@ -185,25 +191,21 @@ def handle_report(self, report): def activate(self, rep_mode=0x37): """ Activates the extension controller and sets the report mode to get data from the gyroscope. - Gets called by the activate method of the Wiimote class. """ - data = 0x04 - reg_addr = 0xA600FE + self.report_type = rep_mode - self.wiimote.memory.write(reg_addr, data) + # Activate MotionPlus as "active" extension. From: http://abstrakraft.org/cwiid/wiki/MotionPlus + self.wiimote.memory.write(0xA600FE, 0x04) self._com.set_report_mode(rep_mode) def deactivate(self): """ Deactivates the extension controller and sets the default report mode. - Gets called by the activate method of the Wiimote class. - """ - data = 0x55 - reg_addr = 0xA400F0 - self.wiimote.memory.write(reg_addr, data) - data = 0x00 - reg_addr = 0xA400FB - self.wiimote.memory.write(reg_addr, data) + """ + + # Deactivate MotionPlus. From http://abstrakraft.org/cwiid/wiki/MotionPlus + self.wiimote.memory.write(0xA400F0, 0x55) + self.wiimote.memory.write(0xA400FB, 0x00) self._com.set_report_mode(0x33) From 0473a2cae7aef4aeeb34e9f20234259ce11bb27b Mon Sep 17 00:00:00 2001 From: wiese4 Date: Tue, 6 Oct 2015 17:49:18 +0200 Subject: [PATCH 3/3] Improved pep8 conformity --- wiimote.py | 87 ++++++++++++++++++++++++++---------------------------- 1 file changed, 42 insertions(+), 45 deletions(-) diff --git a/wiimote.py b/wiimote.py index e610aac..fac82ac 100644 --- a/wiimote.py +++ b/wiimote.py @@ -258,9 +258,9 @@ def _notify_callbacks(self): def handle_report(self, report): """ Extract accelerometer data from a Wiimote report. - Usually gets called by the Wiimote CommunicationHandler object. + Usually gets called by the Wiimote CommunicationHandler object. """ - if report[0] in [0x3e, 0x3f]: # interleaved modes + if report[0] in [0x3e, 0x3f]: # interleaved modes raise NotImplementedError("Data reporting mode 0x3e/0x3f not supported") x_msb, y_msb, z_msb = report[3:6] x = (x_msb << 2) + ((report[1] & 0b01100000) >> 5) @@ -274,10 +274,10 @@ class Buttons(object): """ Represents the buttons of the Wiimote. """ - + BUTTONS = {'A': 0x0008, 'B': 0x0004, - 'Down' : 0x0400, + 'Down': 0x0400, 'Home': 0x0080, 'Left': 0x0100, 'Minus': 0x0010, @@ -286,7 +286,7 @@ class Buttons(object): 'Right': 0x0200, 'Two': 0x0001, 'Up': 0x0800, - } + } def __init__(self, wiimote): self._wiimote = wiimote @@ -310,12 +310,12 @@ def __getitem__(self, btn): def register_callback(self, func): """ - Register a callback function `func` that gets called every time + Register a callback function `func` that gets called every time when new button states are transmitted from the Wiimote. A list of all _changed_ buttons is passed as parameter to this function. """ self._callbacks.append(func) - + def unregister_callback(self, func): """ Unregister a callback function `func` that has been previously registered. @@ -331,7 +331,7 @@ def _notify_callbacks(self, diff): def handle_report(self, report): """ Extract button data from a Wiimote report. - Usually gets called by the Wiimote CommunicationHandler object. + Usually gets called by the Wiimote CommunicationHandler object. """ btn_bytes = (report[1] << 8) + report[2] new_state = {} @@ -347,7 +347,7 @@ def _update_state(self, new_state): diff.append((btn, state)) self._state[btn] = state return diff - + class LEDs(object): """ @@ -377,7 +377,7 @@ def __setitem__(self, led_no, val): self.set_leds(new_led_state) else: raise IndexError("list index out of range") - + def set_leds(self, led_list): """ Set leds 1-4. @@ -389,7 +389,7 @@ def set_leds(self, led_list): led_byte = 0x00 for val, state in zip([0x10, 0x20, 0x40, 0x80], self._state): if state: - led_byte += val + led_byte += val self._com._send(RPT_LED, led_byte) @@ -417,6 +417,7 @@ def rumble(self, length=0.5): t.start() self.set_rumble(True) + class IRCam(object): """ Represents the infrared camera of the Wiimote. @@ -428,15 +429,15 @@ class IRCam(object): # adapted from WiiBrew list, higher index means higher sensitivity SENSITIVITY_BLOCKS = [ - ([0x02, 0x00, 0x00, 0x71, 0x01, 0x00, 0x64, 0x00, 0xfe], [0xfd, 0x05]), # Wii Level 1 - ([0x02, 0x00, 0x00, 0x71, 0x01, 0x00, 0x96, 0x00, 0xb4], [0xb3, 0x04]), # Wii Level 2 - ([0x02, 0x00, 0x00, 0x71, 0x01, 0x00, 0xaa, 0x00, 0x64], [0x63, 0x03]), # Wii Level 3 - ([0x02, 0x00, 0x00, 0x71, 0x01, 0x00, 0xc8, 0x00, 0x36], [0x35, 0x03]), # Wii Level 4 - ([0x07, 0x00, 0x00, 0x71, 0x01, 0x00, 0x72, 0x00, 0x20], [0x1f, 0x03]), # Wii Level 5 - ([0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0xff, 0x00, 0x0c], [0x00, 0x00]), # Max Sensitivity + ([0x02, 0x00, 0x00, 0x71, 0x01, 0x00, 0x64, 0x00, 0xfe], [0xfd, 0x05]), # Wii Level 1 + ([0x02, 0x00, 0x00, 0x71, 0x01, 0x00, 0x96, 0x00, 0xb4], [0xb3, 0x04]), # Wii Level 2 + ([0x02, 0x00, 0x00, 0x71, 0x01, 0x00, 0xaa, 0x00, 0x64], [0x63, 0x03]), # Wii Level 3 + ([0x02, 0x00, 0x00, 0x71, 0x01, 0x00, 0xc8, 0x00, 0x36], [0x35, 0x03]), # Wii Level 4 + ([0x07, 0x00, 0x00, 0x71, 0x01, 0x00, 0x72, 0x00, 0x20], [0x1f, 0x03]), # Wii Level 5 + ([0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0xff, 0x00, 0x0c], [0x00, 0x00]), # Max Sensitivity ] - - SUPPORTED_REPORTS = [0x33, 0x36,0x37,0x3e,0x3f] + + SUPPORTED_REPORTS = [0x33, 0x36, 0x37, 0x3e, 0x3f] def __init__(self, wiimote): self.wiimote = wiimote @@ -473,7 +474,7 @@ def set_mode_sensitivity(self, mode, sensitivity): raise TypeError("wrong mode or sensitivity level given") self._mode = mode self._sensitivity = sensitivity - self._com.set_report_mode(0x33) #todo: adjust for other modes!! + self._com.set_report_mode(0x33) # todo: adjust for other modes!! self._com._send(0x13, 0x04) self._com._send(0x1a, 0x04) self.wiimote.memory.write(0xb00030, 0x08, eeprom=False) @@ -489,14 +490,14 @@ def get_state(self): return self._state def set_sensitivity(self, sensitivity): - self.set_mode_sensitivity(self._mode, sensitivity) + self.set_mode_sensitivity(self._mode, sensitivity) def set_mode(self, mode): - self.set_mode_sensitivity(mode, self._sensitivity) + self.set_mode_sensitivity(mode, self._sensitivity) def register_callback(self, func): self._callbacks.append(func) - + def unregister_callback(self, func): if func in self._callbacks: self._callbacks.remove(func) @@ -524,7 +525,7 @@ class Memory(object): RPT_READ = 0x17 RPT_WRITE = 0x16 - + SUPPORTED_REPORTS = [0x21] MAX_ADDRESS = 0x16FF @@ -536,7 +537,6 @@ def __init__(self, wiimote): self._bytes_requested = 0 self._reply_buffer = [] - def write(self, address, data, eeprom=False): address_bytes = _val_to_byte_list(address, 3, big_endian=True) bytes_to_send = _flatten(data) @@ -551,7 +551,7 @@ def write(self, address, data, eeprom=False): amount_byte = _val_to_byte_list(amount, 1, big_endian=True) bytes_to_send = _add_padding(bytes_to_send, 16) control_or_eeprom = 0x00 if eeprom else 0x04 - self._com._send(Memory.RPT_WRITE, control_or_eeprom, address_bytes, amount_byte, bytes_to_send) + self._com._send(Memory.RPT_WRITE, control_or_eeprom, address_bytes, amount_byte, bytes_to_send) def read(self, address, amount, eeprom=False): if self._request_in_progress: @@ -566,16 +566,16 @@ def read(self, address, amount, eeprom=False): control_or_eeprom = 0x00 if eeprom else 0x04 self._request_in_progress = True self._reply_buffer = [] - self._com._send(Memory.RPT_READ, control_or_eeprom, address_bytes, amount_bytes) + self._com._send(Memory.RPT_READ, control_or_eeprom, address_bytes, amount_bytes) # now wait until handle() has filled our reply buffer while self._request_in_progress: time.sleep(0.01) return self._reply_buffer def handle_report(self, report): - if report[0] not in Memory.SUPPORTED_REPORTS: # interleaved modes + if report[0] not in Memory.SUPPORTED_REPORTS: # interleaved modes raise NotImplementedError("can not handle this report") - error = (report[3] & 0x0f) + error = (report[3] & 0x0f) if error != 0: raise RuntimeError("Error condition %x received during memory read!" % error) num_bytes_received = ((report[3] >> 4) & 0x0f) + 1 @@ -589,18 +589,18 @@ def handle_report(self, report): class CommunicationHandler(threading.Thread): - + MODE_DEFAULT = 0x30 - MODE_ACC = 0x31 + MODE_ACC = 0x31 MODE_EXTENSION = 0x32 - MODE_ACC_IR = 0x33 + MODE_ACC_IR = 0x33 RPT_STATUS_REQ = 0x15 def __init__(self, wiimote): threading.Thread.__init__(self) self.daemon = True - self.rumble = False # rumble always + self.rumble = False # rumble always self.wiimote = wiimote self.btaddr = wiimote.btaddr self.model = wiimote.model @@ -622,14 +622,14 @@ def __init__(self, wiimote): except NotImplementedError: print("socket timeout not implemented with this bluetooth module") self.set_report_mode(self.MODE_ACC_IR) - + def _send(self, *bytes_to_send): _debug("sending " + str(bytes_to_send)) - data_str = self._CMD_SET_REPORT.to_bytes(1,'big') + data_str = self._CMD_SET_REPORT.to_bytes(1, 'big') bytes_to_send = _flatten(bytes_to_send) bytes_to_send[1] |= int(self.rumble) for b in bytes_to_send: - data_str += b.to_bytes(1,'big') + data_str += b.to_bytes(1, 'big') self._sendsocket.send(data_str) def run(self): @@ -639,7 +639,7 @@ def run(self): data = self._datasocket.recv(32) except bluetooth.BluetoothError: continue - if len(data) < 2: # disconnect! + if len(data) < 2: # disconnect! self.running = False else: self._handle(data) @@ -656,7 +656,7 @@ def set_report_mode(self, mode): def _handle(self, bytes_read): _debug("received " + str(bytes_read)) - #assert(bytes_read[0] == self._CMD_SET_REPORT + 1) + # assert(bytes_read[0] == self._CMD_SET_REPORT + 1) rpt_type = bytes_read[1] # all reports include button data self.wiimote.buttons.handle_report(bytes_read[1:]) @@ -691,14 +691,14 @@ def __init__(self, btaddr, model): self.memory = Memory(self) self.ir = IRCam(self) """ - Initializations before this point may not read from memory as + Initializations before this point may not read from memory as this would block forever (until the CommunicationHandler is started). CommunicationHandler can not be started earlier because the sensors would not yet be assigned to variables """ self._com.start() - self.leds[0] = True # set first LED to signal successful connection. - + self.leds[0] = True # set first LED to signal successful connection. + def disconnect(self): self._com.running = False @@ -728,7 +728,4 @@ def set_leds(self, led_list): leds = property(get_leds, set_leds) - - #rumble = property(get_rumble, set_rumble) - - + # rumble = property(get_rumble, set_rumble)