From 5b81e86632edad771022bb0cd058811037669dca Mon Sep 17 00:00:00 2001 From: Andrey Pohilko Date: Wed, 13 Sep 2017 17:43:33 +0300 Subject: [PATCH] Notification reading channel established --- README.md | 27 +++++++------ demo.py | 27 +++++++++++-- pylegoboost/__init__.py | 25 ++++++++++-- pylegoboost/comms.py | 84 ++++++++++++++++++++++++++++------------- 4 files changed, 115 insertions(+), 48 deletions(-) diff --git a/README.md b/README.md index fca7821..7fc213e 100644 --- a/README.md +++ b/README.md @@ -1,21 +1,20 @@ +Best way to start is to look at [demo.py](demo.py) file, or maybe run it. + +# Features + +- auto-detect and connect for Bluetooth device +- permanent Bluetooth connection server for faster debugging +- angled and timed movement for motors +- LED color change + # Ideas -Make a server that will listen to wire proto to speed up debug # Links -https://github.com/JorgePe/BOOSTreveng - -https://github.com/hobbyquaker/node-movehub - -https://github.com/JorgePe/pyb00st - - - -https://github.com/RealTimeWeb/blockpy - -https://ru.wikipedia.org/wiki/App_Inventor - -https://en.wikipedia.org/wiki/Blockly +- https://github.com/JorgePe/BOOSTreveng - source of protocol knowledge +- https://github.com/RealTimeWeb/blockpy +- https://ru.wikipedia.org/wiki/App_Inventor +- https://en.wikipedia.org/wiki/Blockly diff --git a/demo.py b/demo.py index c9e3f98..0bbff79 100644 --- a/demo.py +++ b/demo.py @@ -8,8 +8,7 @@ from pylegoboost.comms import DebugServerConnection, BLEConnection log = logging.getLogger("demo") -def demo_all(conn): - movehub = MoveHub(conn) +def demo_all(movehub): demo_led_colors(movehub) demo_motors_timed(movehub) demo_motors_angled(movehub) @@ -60,6 +59,20 @@ def demo_port_c_motor(movehub): sleep(1) +def vernie_head(movehub): + portd = EncodedMotor(movehub, PORT_D) + while True: + angle = 20 + portd.angled(angle, 0.2) + sleep(2) + portd.angled(angle, -0.2) + sleep(2) + portd.angled(angle, -0.2) + sleep(2) + portd.angled(angle, 0.2) + sleep(2) + + if __name__ == '__main__': logging.basicConfig(level=logging.DEBUG) @@ -69,4 +82,12 @@ if __name__ == '__main__': logging.warning("Failed to use debug server: %s", traceback.format_exc()) connection = BLEConnection().connect() - demo_all(connection) + hub = MoveHub(connection) + sleep(1) + #hub.get_name() + + for x in range(1, 60): + sleep(1) + + # demo_led_colors(hub) + # demo_all(movehub) diff --git a/pylegoboost/__init__.py b/pylegoboost/__init__.py index 02023c6..51c3f90 100644 --- a/pylegoboost/__init__.py +++ b/pylegoboost/__init__.py @@ -14,6 +14,8 @@ class MoveHub(object): """ def __init__(self, connection): + self.notified = False + self.connection = connection self.led = LED(self) @@ -21,15 +23,30 @@ class MoveHub(object): self.motor_B = EncodedMotor(self, PORT_B) self.motor_AB = EncodedMotor(self, PORT_AB) - self.port_c = None - self.port_d = None - # self.button # self.tilt_sensor - # transport.write(ENABLE_NOTIFICATIONS_HANDLE, ENABLE_NOTIFICATIONS_VALUE) + # enables notifications reading + self.connection.set_notify_handler(self._notify) + self.connection.write(ENABLE_NOTIFICATIONS_HANDLE, ENABLE_NOTIFICATIONS_VALUE) + + # while not self.notified: + # log.debug("Waiting to be notified") + # time.sleep(1) + + # self.port_C = None + # self.port_D = None + # transport.write(MOVE_HUB_HARDWARE_HANDLE, b'\x0a\x00\x41\x01\x08\x01\x00\x00\x00\x01') + def _notify(self, handle, data): + # TODO + log.debug("Notification on %s: %s", handle, data.encode("hex")) + + def get_name(self): + # note: reading this too fast makes it hang + self.connection.read(DEVICE_NAME) + class Peripheral(object): """ diff --git a/pylegoboost/comms.py b/pylegoboost/comms.py index 80bf274..9856c68 100644 --- a/pylegoboost/comms.py +++ b/pylegoboost/comms.py @@ -4,9 +4,11 @@ This package holds communication aspects import json import logging import socket +import time import traceback from abc import abstractmethod from gattlib import DiscoveryService, GATTRequester +from threading import Thread from pylegoboost.constants import DEVICE_NAME, LEGO_MOVE_HUB @@ -25,6 +27,7 @@ class Requester(GATTRequester): self.notification_sink = None def on_notification(self, handle, data): + # log.debug("requester notified, sink: %s", self.notification_sink) if self.notification_sink: self.notification_sink(handle, data) @@ -42,7 +45,7 @@ class Connection(object): pass @abstractmethod - def notify(self, handle, data): + def set_notify_handler(self, handler): pass @@ -51,10 +54,6 @@ class ConnectionMock(Connection): For unit testing purposes """ - def notify(self, handle, data): - # TODO - pass - def write(self, handle, data): log.debug("Writing to %s: %s", handle, data.encode("hex")) @@ -66,13 +65,13 @@ class ConnectionMock(Connection): class BLEConnection(Connection): """ Main transport class, uses real Bluetooth LE connection. - Loops with timeout of 5 seconds to find device named "Lego MOVE Hub" + Loops with timeout of 1 seconds to find device named "Lego MOVE Hub" :type requester: Requester """ def __init__(self): - super(Connection, self).__init__() + super(BLEConnection, self).__init__() self.requester = None def connect(self, bt_iface_name='hci0'): @@ -80,7 +79,7 @@ class BLEConnection(Connection): while not self.requester: log.info("Discovering devices using %s...", bt_iface_name) - devices = service.discover(5) + devices = service.discover(1) log.debug("Devices: %s", devices) for address, name in devices.items(): @@ -94,7 +93,13 @@ class BLEConnection(Connection): def _get_requester(self, address, bt_iface_name): self.requester = Requester(address, True, bt_iface_name) - self.requester.notification_sink = self.notify + + def set_notify_handler(self, handler): + if self.requester: + log.debug("Setting notification handler: %s", handler) + self.requester.notification_sink = handler + else: + raise RuntimeError("No requester available") def read(self, handle): log.debug("Reading from: %s", handle) @@ -108,10 +113,6 @@ class BLEConnection(Connection): log.debug("Writing to %s: %s", handle, data.encode("hex")) return self.requester.write_by_handle(handle, data) - def notify(self, handle, data): - # TODO - log.debug("Notification on %s: %s", handle, data.encode("hex")) - class DebugServer(object): """ @@ -133,14 +134,26 @@ class DebugServer(object): while True: log.info("Accepting connections at %s", port) conn, addr = self.sock.accept() + self.ble.requester.notification_sink = lambda x, y: self._notify(conn, x, y) try: self._handle_conn(conn) + except BaseException: + log.error("Problem handling incoming connection: %s", traceback.format_exc()) finally: + self.ble.requester.notification_sink = None conn.close() def __del__(self): self.sock.close() + def _notify(self, conn, handle, data): + payload = {"type": "notification", "handle": handle, "data": data.encode('hex')} + log.debug("Send notification: %s", payload) + try: + conn.send(json.dumps(payload) + "\n") + except BaseException: + log.error("Problem sending notification: %s", traceback.format_exc()) + def _handle_conn(self, conn): """ :type conn: socket._socketobject @@ -165,13 +178,14 @@ class DebugServer(object): except BaseException: log.error("Failed to handle cmd: %s", traceback.format_exc()) - # conn.send(data.upper()) - def _handle_cmd(self, cmd): if cmd['type'] == 'write': self.ble.write(cmd['handle'], cmd['data'].decode('hex')) elif cmd['type'] == 'read': - self.sock.send(self.ble.read(cmd['handle']).encode('hex') + "\n") + data = self.ble.read(cmd['handle']) + payload = {"type": "response", "data": data.encode('hex')} + log.debug("Send response: %s", payload) + self.sock.send(json.dumps(payload) + "\n") else: raise ValueError("Unhandled cmd: %s", cmd) @@ -181,18 +195,21 @@ class DebugServerConnection(Connection): Connection type to be used with DebugServer, replaces BLEConnection """ - def __init__(self): + def __init__(self, port=9090): + super(DebugServerConnection, self).__init__() + self.notify_handler = None self.buf = "" self.sock = socket.socket() - self.sock.connect(('localhost', 9090)) + self.sock.connect(('localhost', port)) + self.incoming = [] + + self.reader = Thread(target=self._recv) + self.reader.setDaemon(True) + self.reader.start() def __del__(self): self.sock.close() - def notify(self, handle, data): - # TODO - pass - def write(self, handle, data): payload = { "type": "write", @@ -207,7 +224,13 @@ class DebugServerConnection(Connection): "handle": handle } self._send(payload) - return self._recv() + + while True: + for item in self.incoming: + if item['type'] == 'response': + self.incoming.remove(item) + return item['data'].decode('hex') + time.sleep(0.1) def _send(self, payload): log.debug("Sending to debug server: %s", payload) @@ -222,10 +245,17 @@ class DebugServerConnection(Connection): self.buf += data - if "\n" in self.buf: + while "\n" in self.buf: line = self.buf[:self.buf.index("\n")] self.buf = self.buf[self.buf.index("\n") + 1:] if line: - return line.decode("hex") - break - raise RuntimeError("No data read") + item = json.loads(line) + if item['type'] == 'notification' and self.notify_handler: + self.notify_handler(item['handle'], item['data'].decode('hex')) + elif item['type'] == 'response': + self.incoming.append(item) + else: + log.warning("Dropped inbound: %s", item) + + def set_notify_handler(self, handler): + self.notify_handler = handler