Skip to content

Implemented support for gyroscope #8

New issue

Have a question about this project? Sign up for a free GitHub account to open an issue and contact its maintainers and the community.

Sign up for GitHub

By clicking “Sign up for GitHub”, you agree to our terms of service and privacy statement. We’ll occasionally send you account related emails.

Already on GitHub? Sign in to your account

Open
miji-dev wants to merge 3 commits into
base: master
Choose a base branch
from
Open

Implemented support for gyroscope #8

Changes from all commits
Commits
Show all changes
3 commits
Select commit Hold shift + click to select a range
e56e030
Added Gyroscope class
miji-dev Oct 6, 2015
fcb6aea
Refactored Gyroscope class
miji-dev Oct 6, 2015
0473a2c
Improved pep8 conformity
miji-dev Oct 6, 2015
File filter

Filter by extension

Filter by extension
Conversations
Failed to load comments.
Loading
Jump to
Jump to file
Failed to load files.
Loading
Diff view
Diff view
181 changes: 136 additions & 45 deletions wiimote.py
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -119,6 +119,96 @@ 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.
"""

# 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()

def activate(self, rep_mode=0x37):
"""
Activates the extension controller and sets the report mode to get data from the gyroscope.
"""

self.report_type = rep_mode
# 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.
"""

# 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)


class Accelerometer(object):
"""
Represents the accelerometer of the Wiimote.
Expand Down Expand Up @@ -168,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)
Expand All @@ -184,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,
Expand All @@ -196,7 +286,7 @@ class Buttons(object):
'Right': 0x0200,
'Two': 0x0001,
'Up': 0x0800,
}
}

def __init__(self, wiimote):
self._wiimote = wiimote
Expand All @@ -220,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.
Expand All @@ -241,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 = {}
Expand All @@ -257,7 +347,7 @@ def _update_state(self, new_state):
diff.append((btn, state))
self._state[btn] = state
return diff


class LEDs(object):
"""
Expand Down Expand Up @@ -287,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.
Expand All @@ -299,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)


Expand Down Expand Up @@ -327,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.
Expand All @@ -338,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
Expand Down Expand Up @@ -383,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)
Expand All @@ -399,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)
Expand Down Expand Up @@ -434,7 +525,7 @@ class Memory(object):

RPT_READ = 0x17
RPT_WRITE = 0x16

SUPPORTED_REPORTS = [0x21]

MAX_ADDRESS = 0x16FF
Expand All @@ -446,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)
Expand All @@ -461,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:
Expand All @@ -476,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
Expand All @@ -499,17 +589,18 @@ def handle_report(self, report):


class CommunicationHandler(threading.Thread):

MODE_DEFAULT = 0x30
MODE_ACC = 0x31
MODE_ACC_IR = 0x33
MODE_ACC = 0x31
MODE_EXTENSION = 0x32
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
Expand All @@ -531,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):
Expand All @@ -548,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)
Expand All @@ -565,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:])
Expand All @@ -575,6 +666,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
Expand All @@ -592,19 +685,20 @@ 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)
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

Expand Down Expand Up @@ -634,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)