From 1619868af74d6fc9e6454606055f1fc3ccd99c49 Mon Sep 17 00:00:00 2001 From: Andrey Pohilko Date: Thu, 14 Sep 2017 12:30:17 +0300 Subject: [PATCH] Working on tilt sensors --- demo.py | 10 ++++++---- pylgbst/__init__.py | 32 +++++++++++++++++++++----------- pylgbst/comms.py | 6 ++++++ pylgbst/constants.py | 5 +++-- pylgbst/peripherals.py | 10 +++++++--- test.py => tests.py | 23 +++++++++++++++-------- 6 files changed, 58 insertions(+), 28 deletions(-) rename test.py => tests.py (86%) diff --git a/demo.py b/demo.py index 109fdbf..87d932d 100644 --- a/demo.py +++ b/demo.py @@ -98,10 +98,12 @@ if __name__ == '__main__': connection = BLEConnection().connect() hub = MoveHub(connection) - demo_all(hub) + hub.tilt_sensor.subscribe(lambda: log.info("Tilt")) + sleep(60) + # demo_all(hub) - #sleep(1) + # sleep(1) # hub.get_name() - #demo_port_cd_motor(hub) + # demo_port_cd_motor(hub) # demo_led_colors(hub) - #sleep(1) + # sleep(1) diff --git a/pylgbst/__init__.py b/pylgbst/__init__.py index 8dc0e17..a4fd857 100644 --- a/pylgbst/__init__.py +++ b/pylgbst/__init__.py @@ -40,11 +40,11 @@ class MoveHub(object): self.port_C = None self.port_D = None + self.connection.set_notify_handler(self._notify) + self._wait_for_devices() def _wait_for_devices(self): - # enables notifications reading - self.connection.set_notify_handler(self._notify) self.connection.write(ENABLE_NOTIFICATIONS_HANDLE, ENABLE_NOTIFICATIONS_VALUE) builtin_devices = () @@ -66,19 +66,29 @@ class MoveHub(object): Using https://github.com/JorgePe/BOOSTreveng/blob/master/Notifications.md """ orig = data - data = data[3:] - log.debug("Notification on %s: %s", handle, str2hex(orig)) - msg_type = get_byte(data, 2) + if handle == MOVE_HUB_HARDWARE_HANDLE: + data = data[3:] + log.debug("Notification on %s: %s", handle, str2hex(orig)) - if msg_type == MSG_PORT_INFO: - self._handle_port_info(data) - elif msg_type == MSG_PORT_STATUS: - self._handle_port_status(data) + msg_type = get_byte(data, 2) + + if msg_type == MSG_PORT_INFO: + self._handle_port_info(data) + elif msg_type == MSG_PORT_STATUS: + self._handle_port_status(data) + elif msg_type == MSG_SENSOR_DATA: + self._handle_sensor_data(data) + else: + log.warning("Unhandled msg type 0x%x: %s", msg_type, str2hex(orig)) else: - log.warning("Unhandled msg type 0x%x: %s", msg_type, str2hex(orig)) + log.warning("Unsupported notification handle: 0x%s", handle) - pass + def _handle_sensor_data(self, data): + port = get_byte(data, 3) + sensor = self.devices[port] + if isinstance(sensor, TiltSensor): + sensor.notify_subscribers() def _handle_port_status(self, data): port = get_byte(data, 3) diff --git a/pylgbst/comms.py b/pylgbst/comms.py index f017e9f..792fe01 100644 --- a/pylgbst/comms.py +++ b/pylgbst/comms.py @@ -151,6 +151,8 @@ class DebugServer(object): self.ble.requester.notification_sink = lambda x, y: self._notify(conn, x, y) try: self._handle_conn(conn) + except KeyboardInterrupt: + raise except BaseException: log.error("Problem handling incoming connection: %s", traceback.format_exc()) finally: @@ -165,6 +167,8 @@ class DebugServer(object): log.debug("Send notification: %s", payload) try: conn.send(json.dumps(payload) + "\n") + except KeyboardInterrupt: + raise except BaseException: log.error("Problem sending notification: %s", traceback.format_exc()) @@ -189,6 +193,8 @@ class DebugServer(object): log.info("Cmd line: %s", line) try: self._handle_cmd(json.loads(line)) + except KeyboardInterrupt: + raise except BaseException: log.error("Failed to handle cmd: %s", traceback.format_exc()) diff --git a/pylgbst/constants.py b/pylgbst/constants.py index 311a927..46bcbc6 100644 --- a/pylgbst/constants.py +++ b/pylgbst/constants.py @@ -60,9 +60,10 @@ PORTS = { # PACKET TYPES MSG_PORT_INFO = 0x04 -MSG_PORT_STATUS = 0x82 MSG_SET_PORT_VAL = 0x81 -MSG_PORT_SUBSCRIBE = 0x41 +MSG_PORT_STATUS = 0x82 +MSG_SENSOR_SUBSCRIBE = 0x41 +MSG_SENSOR_DATA = 0x45 # NOTIFICATIONS TYPE_DISTANCE_COLOR_SENSOR = 0x25 diff --git a/pylgbst/peripherals.py b/pylgbst/peripherals.py index 9bc5dca..8db10a0 100644 --- a/pylgbst/peripherals.py +++ b/pylgbst/peripherals.py @@ -34,7 +34,7 @@ class Peripheral(object): def _subscribe_on_port(self, params): # FIXME: became obsolete - self._write_to_hub(MSG_PORT_SUBSCRIBE, params) + self._write_to_hub(MSG_SENSOR_SUBSCRIBE, params) def started(self): self.working = True @@ -42,6 +42,10 @@ class Peripheral(object): def finished(self): self.working = False + def notify_subscribers(self, *args, **kwargs): + for subscriber in self._subscribers: + subscriber(*args, **kwargs) + class LED(Peripheral): def set_color(self, color): @@ -122,8 +126,8 @@ class ColorDistanceSensor(Peripheral): class TiltSensor(Peripheral): def subscribe(self, callback): - params = b'\x00\x01\x00\x00\x00\x01' # full - # params = b'\x02\x01\x00\x00\x00\x01' # basic + #params = b'\x00\x01\x00\x00\x00\x01' # full + params = b'\x02\x01\x00\x00\x00\x01' # basic self._subscribe_on_port(params) self._subscribers.append(callback) # TODO: maybe join it into `_subscribe_on_port` diff --git a/test.py b/tests.py similarity index 86% rename from test.py rename to tests.py index a8b00f5..80ec614 100644 --- a/test.py +++ b/tests.py @@ -5,8 +5,7 @@ from threading import Thread from pylgbst import MoveHub, COLOR_RED, LED, EncodedMotor, PORT_AB from pylgbst.comms import Connection, str2hex, hex2str -from pylgbst.constants import PORT_LED, PORT_TILT_SENSOR -from pylgbst.peripherals import TiltSensor +from pylgbst.constants import PORT_LED logging.basicConfig(level=logging.DEBUG) @@ -60,6 +59,13 @@ class HubMock(MoveHub): class GeneralTest(unittest.TestCase): + def _wait_notifications_handled(self, hub): + hub.connection.running = False + for _ in range(1, 1000): + time.sleep(0.1) + if hub.connection.finished: + break + def test_led(self): hub = HubMock() led = LED(hub, PORT_LED) @@ -68,13 +74,17 @@ class GeneralTest(unittest.TestCase): def test_tilt_sensor(self): hub = HubMock() - hub.tilt_sensor = TiltSensor(hub, PORT_TILT_SENSOR) + hub.connection.notifications.append((14, '1b0e00 0f00 04 3a 0128000000000100000001')) + time.sleep(1) def callback(): - pass + log.debug("Tilt") hub.tilt_sensor.subscribe(callback) + hub.connection.notifications.append((14, "1b0e000600453a04fe")) + time.sleep(10) self.assertEquals("0a01413a000100000001", hub.connection.writes[0][1]) + self._wait_notifications_handled(hub) def test_motor(self): conn = ConnectionMock() @@ -102,7 +112,4 @@ class GeneralTest(unittest.TestCase): hub = MoveHub(conn) # demo_all(hub) - conn.running = False - - while not conn.finished: - time.sleep(0.1) + self._wait_notifications_handled(hub)