import threading import numpy as np from constants import * from bleak import BleakClient from uuid import uuid4 class Wand(object): """A wand class to interact with the Kano wand """ def __init__(self, device_addr, name, debug=True): """Create a new wand Arguments: device {bluepy.ScanEntry} -- Device information Keyword Arguments: debug {bool} -- Print debug messages (default: {False}) """ super().__init__(None) self.debug = debug self._dev = BleakClient(device_addr) self.name = name if debug: print("Wand: {}\n\rWand Mac: {}".format(self.name, self._dev.address)) # Notification stuff self.connected = False self._position_callbacks = {} self._position_subscribed = False self._button_callbacks = {} self._button_subscribed = False self._temperature_callbacks = {} self._temperature_subscribed = False self._battery_callbacks = {} self._battery_subscribed = False self._notification_thread = None self._position_notification_handle = 41 self._button_notification_handle = 33 self._temp_notification_handle = 56 self._battery_notification_handle = 23 async def connect(self): if self.debug: print("Connecting to {}...".format(self.name)) connected = await self._dev.connect() if not connected: raise Exception("ERROR NOT CONNECTED TO THE DEVICE") self.connected = True self.post_connect() if self.debug: print("Connected to {}".format(self.name)) def post_connect(self): """Do anything necessary after connecting """ pass async def disconnect(self): await self._dev.disconnect() self.connected = False self.post_disconnect() if self.debug: print("Disconnected from {}".format(self.name)) def post_disconnect(self): """Do anything necessary after disconnecting """ pass # def get_organization(self): # """Get organization of device # # Returns {str} -- Organization name # """ # # with self._lock: # if not hasattr(self, "_organization_handle"): # handle = self._info_service.getCharacteristics(INFO.ORGANIZATION_CHAR.value)[0] # self._organization_handle = handle.getHandle() # return self.readCharacteristic(self._organization_handle).decode("utf-8") # def get_software_version(self): # # """Get software version # # Returns {str} -- Version number # """ # #with self._lock: # if not hasattr(self, "_software_handle"): # handle = self._info_service.getCharacteristics(INFO.SOFTWARE_CHAR.value)[0] # self._software_handle = handle.getHandle() # return self.readCharacteristic(self._software_handle).decode("utf-8") # def get_hardware_version(self): # # """Get hardware version # # Returns {str} -- Hardware version # """ # #with self._lock: # if not hasattr(self, "_hardware_handle"): # handle = self._info_service.getCharacteristics(INFO.HARDWARE_CHAR.value)[0] # self._hardware_handle = handle.getHandle() # return self.readCharacteristic(self._hardware_handle).decode("utf-8") # # # def get_battery(self): # """Get battery level (currently only returns 0) # # Returns {str} -- Battery level # """ # # with self._lock: # if not hasattr(self, "_battery_handle"): # handle = self._io_service.getCharacteristics(IO.BATTERY_CHAR.value)[0] # self._battery_handle = handle.getHandle() # return self.readCharacteristic(self._battery_handle).decode("utf-8") async def get_button(self): """Get current button status Returns {bool} -- Button pressed status """ # with self._lock: # if not hasattr(self, "_button_handle"): # handle = self._io_service.getCharacteristics(IO.USER_BUTTON_CHAR.value)[0] # self._button_handle = handle.getHandle() # data = self.readCharacteristic(self._button_handle) data = await self._dev.read_gatt_char(IO.USER_BUTTON_CHAR.value) return data[0] == 1 async def get_temperature(self): """Get temperature Returns {str} -- Battery level """ # with self._lock: # if not hasattr(self, "_temperature_handle"): # handle = self._sensor_service.getCharacteristics(SENSOR.TEMP_CHAR.value)[0] # self._temperature_handle = handle.getHandle() return await self._dev.read_gatt_char(SENSOR.TEMP_CHAR.value) async def keep_alive(self): """Keep the wand's connection active Returns {bytes} -- Status """ # Is not documented because it doesn't seem to work? if self.debug: print("Keeping wand alive.") # with self._lock: # if not hasattr(self, "_alive_handle"): # handle = self._io_service.getCharacteristics(IO.KEEP_ALIVE_CHAR.value)[0] # self._alive_handle = handle.getHandle() return await self._dev.write_gatt_char(IO.KEEP_ALIVE_CHAR.value, bytes([1]), response=True) # writeCharacteristic(self._alive_handle, bytes([1]), withResponse=True) async def vibrate(self, pattern=PATTERN.REGULAR): """Vibrate wand with pattern Keyword Arguments: pattern {kano_wand.PATTERN} -- Vibration pattern (default: {PATTERN.REGULAR}) Returns {bytes} -- Status """ # with self._lock: if isinstance(pattern, PATTERN): message = [pattern.value] else: message = [pattern] if self.debug: print("Setting VibrationLed to {}".format(message)) # # if not hasattr(self, "_vibrator_handle"): # handle = self._io_service.getCharacteristics(IO.VIBRATOR_CHAR.value)[0] # self._vibrator_handle = handle.getHandle() # self.writeCharacteristic(self._vibrator_handle, bytes(message), withResponse=True) return await self._dev.write_gatt_char(IO.VIBRATOR_CHAR.value, bytes(message), response=True) async def set_led(self, color="0x2185d0", on=True): """Set the LED's color Keyword Arguments: color {str} -- Color hex code (default: {"0x2185d0"}) on {bool} -- Whether light is on or off (default: {True}) Returns {bytes} -- Status """ message = [] if on: message.append(1) else: message.append(0) # I got this from Kano's node module color = int(color.replace("#", ""), 16) r = (color >> 16) & 255 g = (color >> 8) & 255 b = color & 255 rgb = (((r & 248) << 8) + ((g & 252) << 3) + ((b & 248) >> 3)) message.append(rgb >> 8) message.append(rgb & 0xff) if self.debug: print("Setting LED to {}".format(message)) # with self._lock: # if not hasattr(self, "_led_handle"): # handle = self._io_service.getCharacteristics(IO.LED_CHAR.value)[0] # self._led_handle = handle.getHandle() # self.writeCharacteristic(self._led_handle, bytes(message), withResponse=True) return await self._dev.write_gatt_char(IO.LED_CHAR.value, bytes(message), response=True) # SENSORS # def on(self, event, callback): # """Add an event listener # # Arguments: # event {str} -- Event type, "position", "button", "temp", or "battery" # callback {function} -- Callback function # # Returns {str} -- ID of the callback for removal later # """ # if self.debug: # print("Adding callback for {} notification...".format(event)) # # id = None # if event == "position": # id = uuid4() # self._position_callbacks[id] = callback # self.subscribe_position() # elif event == "button": # id = uuid4() # self._button_callbacks[id] = callback # self.subscribe_button() # elif event == "temp": # id = uuid4() # self._temperature_callbacks[id] = callback # self.subscribe_temperature() # elif event == "battery": # id = uuid4() # self._battery_callbacks[id] = callback # self.subscribe_battery() # # return id # # # def off(self, uuid, continue_notifications=False): # """Remove a callback # # Arguments: # uuid {str} -- Remove a callback with its id # # Keyword Arguments: # continue_notifications {bool} -- Keep notification thread running (default: {False}) # # Returns {bool} -- If removal was successful or not # """ # removed = False # if self._position_callbacks.get(uuid) != None: # removed = True # self._position_callbacks.pop(uuid) # if len(self._position_callbacks.values()) == 0: # self.unsubscribe_position(continue_notifications=continue_notifications) # elif self._button_callbacks.get(uuid) != None: # removed = True # self._button_callbacks.pop(uuid) # if len(self._button_callbacks.values()) == 0: # self.unsubscribe_button(continue_notifications=continue_notifications) # elif self._temperature_callbacks.get(uuid) != None: # removed = True # self._temperature_callbacks.pop(uuid) # if len(self._temperature_callbacks.values()) == 0: # self.unsubscribe_temperature(continue_notifications=continue_notifications) # elif self._battery_callbacks.get(uuid) != None: # removed = True # self._battery_callbacks.pop(uuid) # if len(self._battery_callbacks.values()) == 0: # self.unsubscribe_battery(continue_notifications=continue_notifications) # # if self.debug: # if removed: # print("Removed callback {}".format(uuid)) # else: # print("Could not remove callback {}".format(uuid)) # # return removed # def subscribe_position(self): # """Subscribe to position notifications and start thread if necessary # """ # if self.debug: # print("Subscribing to position notification") # # self._position_subscribed = True # # with self._lock: # if not hasattr(self, "_position_handle"): # handle = self._sensor_service.getCharacteristics(SENSOR.QUATERNIONS_CHAR.value)[0] # self._position_handle = handle.getHandle() # # self.writeCharacteristic(self._position_handle + 1, bytes([1, 0])) # # # self._start_notification_thread() # # # def unsubscribe_position(self, continue_notifications=False): # """Unsubscribe to position notifications # # Keyword Arguments: # continue_notifications {bool} -- Keep notification thread running (default: {False}) # """ # if self.debug: # print("Unsubscribing from position notification") # # self._position_subscribed = continue_notifications # # with self._lock: # if not hasattr(self, "_position_handle"): # handle = self._sensor_service.getCharacteristics(SENSOR.QUATERNIONS_CHAR.value)[0] # self._position_handle = handle.getHandle() # # self.writeCharacteristic(self._position_handle + 1, bytes([0, 0])) async def subscribe_button(self): """Subscribe to button notifications and start thread if necessary """ if self.debug: print("Subscribing to button notification") self._button_subscribed = True await self._dev.start_notify(IO.USER_BUTTON_CHAR.value, self.handle_notification) # with self._lock: # if not hasattr(self, "_button_handle"): # handle = self._io_service.getCharacteristics(IO.USER_BUTTON_CHAR.value)[0] # self._button_handle = handle.getHandle() # self.writeCharacteristic(self._button_handle + 1, bytes([1, 0])) # self._start_notification_thread() async def unsubscribe_button(self, continue_notifications=False): """Unsubscribe to button notifications Keyword Arguments: continue_notifications {bool} -- Keep notification thread running (default: {False}) """ if self.debug: print("Unsubscribing from button notification") self._button_subscribed = continue_notifications # with self._lock: # if not hasattr(self, "_button_handle"): # handle = self._io_service.getCharacteristics(IO.USER_BUTTON_CHAR.value)[0] # self._button_handle = handle.getHandle() # # self.writeCharacteristic(self._button_handle + 1, bytes([0, 0])) await self._dev.stop_notify(IO.USER_BUTTON_CHAR.value) # def subscribe_temperature(self): # """Subscribe to temperature notifications and start thread if necessary # """ # if self.debug: # print("Subscribing to temperature notification") # # self._temperature_subscribed = True # # with self._lock: # if not hasattr(self, "_temp_handle"): # handle = self._sensor_service.getCharacteristics(SENSOR.TEMP_CHAR.value)[0] # self._temp_handle = handle.getHandle() # # self.writeCharacteristic(self._temp_handle + 1, bytes([1, 0])) # # self._start_notification_thread() # # # def unsubscribe_temperature(self, continue_notifications=False): # """Unsubscribe to temperature notifications # # Keyword Arguments: # continue_notifications {bool} -- Keep notification thread running (default: {False}) # """ # if self.debug: # print("Unsubscribing from temperature notification") # # self._temperature_subscribed = continue_notifications # # with self._lock: # if not hasattr(self, "_temp_handle"): # handle = self._sensor_service.getCharacteristics(SENSOR.TEMP_CHAR.value)[0] # self._temp_handle = handle.getHandle() # # self.writeCharacteristic(self._temp_handle + 1, bytes([0, 0])) # # # def subscribe_battery(self): # """Subscribe to battery notifications and start thread if necessary # """ # if self.debug: # print("Subscribing to battery notification") # # self._battery_subscribed = True # # with self._lock: # if not hasattr(self, "_battery_handle"): # handle = self._io_service.getCharacteristics(IO.BATTERY_CHAR.value)[0] # self._battery_handle = handle.getHandle() # # self.writeCharacteristic(self._battery_handle + 1, bytes([1, 0])) # self._start_notification_thread() # # # def unsubscribe_battery(self, continue_notifications=False): # """Unsubscribe to battery notifications # # Keyword Arguments: # continue_notifications {bool} -- Keep notification thread running (default: {False}) # """ # if self.debug: # print("Unsubscribing from battery notification") # # self._battery_subscribed = continue_notifications # # with self._lock: # if not hasattr(self, "_battery_handle"): # handle = self._io_service.getCharacteristics(IO.BATTERY_CHAR.value)[0] # self._battery_handle = handle.getHandle() # # self.writeCharacteristic(self._battery_handle + 1, bytes([0, 0])) # # # def _start_notification_thread(self): # try: # if self._notification_thread == None: # self.reset_position() # self._notification_thread = threading.Thread(target=self._notification_wait) # self._notification_thread.start() # except: # pass # def _notification_wait(self): # if self.debug: # print("Notification thread started") # # while (self.connected and # (self._position_subscribed or # self._button_subscribed or # self._temperature_subscribed or # self._battery_subscribed)): # try: # if super().waitForNotifications(1): # continue # except: # continue # # if self.debug: # print("Notification thread stopped") def _on_position(self, data): """Private function for position notification Arguments: data {bytes} -- Data from device """ # I got part of this from Kano's node module and modified it y = np.int16(np.uint16(int.from_bytes(data[0:2], byteorder='little'))) x = -1 * np.int16(np.uint16(int.from_bytes(data[2:4], byteorder='little'))) w = -1 * np.int16(np.uint16(int.from_bytes(data[4:6], byteorder='little'))) z = np.int16(np.uint16(int.from_bytes(data[6:8], byteorder='little'))) if self.debug: pitch = "Pitch: {}".format(z).ljust(16) roll = "Roll: {}".format(w).ljust(16) print("{}{}(x, y): ({}, {})".format(pitch, roll, x, y)) self.on_position(x, y, z, w) for callback in self._position_callbacks.values(): callback(x, y, z, w) def on_position(self, roll, x, y, z): """Function called on position notification Arguments: x {int} -- X position of wand (Between -1000 and 1000) y {int} -- Y position of wand (Between -1000 and 1000) pitch {int} -- Pitch of wand (Between -1000 and 1000) roll {int} -- Roll of wand (Between -1000 and 1000) """ pass # def reset_position(self): # """Reset the quaternains of the wand # """ # handle = self._sensor_service.getCharacteristics(SENSOR.QUATERNIONS_RESET_CHAR.value)[0].getHandle() # # with self._lock: # self.writeCharacteristic(handle, bytes([1])) def _on_button(self, data): """Private function for button notification Arguments: data {bytes} -- Data from device """ val = data[0] == 1 if self.debug: print("Button: {}".format(val)) self.on_button(val) for callback in self._button_callbacks.values(): callback(val) def on_button(self, value): """Function called on button notification Arguments: pressed {bool} -- If button is pressed """ pass def _on_temperature(self, data): """Private function for temperature notification Arguments: data {bytes} -- Data from device """ val = np.int16(np.uint16(int.from_bytes(data[0:2], byteorder='little'))) if self.debug: print("Temperature: {}".format(val)) self.on_temperature(val) for callback in self._temperature_callbacks.values(): callback(val) def on_temperature(self, value): """Function called on temperature notification Arguments: value {int} -- Temperature of the wand """ pass def _on_battery(self, data): """Private function for battery notification Arguments: data {bytes} -- Data from device """ val = data[0] if self.debug: print("Battery: {}".format(val)) self.on_battery(val) for callback in self._battery_callbacks.values(): callback(val) def on_battery(self, value): """Function called on battery notification Arguments: value {int} -- Battery level of the wand """ pass def handle_notification(self, sender, data): """Handle notifications subscribed to Arguments: cHandle {int} -- Handle of notification data {bytes} -- Data from device """ if sender == SENSOR.QUATERNIONS_CHAR.value: self._on_position(data) elif sender == IO.USER_BUTTON_CHAR.value: self._on_button(data) elif sender == SENSOR.TEMP_CHAR.value: self._on_temperature(data) elif sender == IO.BATTERY_CHAR.value: self._on_battery(data)