Fix UT

Sometimes the data returnes is not byte/bytes, but native objective-c class _NSInlineData.
Seems to be a bleak bug, just convert it to bytes as workaround for now.

Co-authored-by: Dmitry Fink <finik@dishero.com>

.gitignore

@@ -3,3 +3,5 @@
 *.pyc
 build
 *.avi
+test_real.py
+.vscode/settings.json

.travis.yml

@@ -1,37 +1,37 @@
 sudo: false
 language: python
-virtualenv:
-  system_site_packages: true
-
-matrix:
-  include:
-  - os: linux
-    python: 2.7
-  - os: linux
-    python: 3.4
+python:
+  - 3.6
+  - 3.8
 
 addons:
   apt:
     packages:
-    - libboost-python-dev
-    - libboost-thread-dev
-    - libbluetooth-dev
+      - libboost-python-dev
+      - libboost-thread-dev
+      - libbluetooth-dev
 
-    - libglib2.0-dev
-    - libdbus-1-dev
-    - libdbus-glib-1-dev
-    - libgirepository-1.0-1
+      - libglib2.0-dev
+      - libdbus-1-dev
+      - libdbus-glib-1-dev
+      - libgirepository-1.0-1
+      - libgirepository1.0-dev
 
-    - python-dbus
-    - python-gi
-    - python3-dbus
-    - python3-gi
+      - bluez
 install:
-- pip install codecov nose-exclude gattlib pygatt gatt pexpect
+  - wget https://github.com/labapart/gattlib/releases/download/dev/gattlib_dbus_0.2-dev_x86_64.deb
+  - sudo dpkg -i gattlib_dbus_0.2-dev_x86_64.deb
+  - pip install codecov codacy-coverage pytest pygatt gatt pexpect bluepy bleak packaging dbus-python pygobject
+  - pip install --upgrade attrs
 
+env:
+  - READTHEDOCS=True
 
-script: coverage run --source=. `which nosetests` tests --nocapture --exclude-dir=examples
+script:
+  - coverage run --omit="examples/*" --source=. -m pytest -v --ignore=examples --log-level=INFO tests
 
 after_success:
-- coverage report -m
-- codecov
+  - coverage report -m
+  - coverage xml
+  - codecov
+  - python-codacy-coverage -r coverage.xml

README.md

@@ -1,262 +1,67 @@
-from pylgbst.comms_gatt import GattConnection# Python library to interact with Move Hub
+# Python library to interact with Move Hub / PoweredUp Hubs
 
-_Move Hub is central controller block of [LEGO® Boost Robotics Set](https://www.lego.com/en-us/boost)._
+_Move Hub is central controller block of [LEGO® Boost Robotics Set](https://www.lego.com/themes/boost)._
 
-In fact, Move Hub is just Bluetooth hardware, all manipulations are done with commands passed through Bluetooth Low Energy (BLE) wireless protocol. One of ways to issue these commands is to write Python program using this library.
+In fact, Move Hub is just a Bluetooth hardware piece, and all manipulations with it are made by commands passed through Bluetooth Low Energy (BLE) wireless protocol. One of the ways to issue these commands is to write Python program using this library.
 
-Best way to start is to look into [`demo.py`](examples/demo.py) file, and run it (assuming you have installed library).
+The best way to start is to look into [`demo.py`](examples/demo.py) file, and run it (assuming you have installed library).
 
 If you have Vernie assembled, you might run scripts from [`examples/vernie`](examples/vernie) directory.
 
-Demonstrational videos:
+## Demonstrational Videos
 
 [![Vernie Programmed](http://img.youtube.com/vi/oqsmgZlVE8I/0.jpg)](http://www.youtube.com/watch?v=oqsmgZlVE8I)
 [![Laser Engraver](http://img.youtube.com/vi/ZbKmqVBBMhM/0.jpg)](https://youtu.be/ZbKmqVBBMhM)
 [![Color Sorter](http://img.youtube.com/vi/829RKT8v8M0/0.jpg)](https://youtu.be/829RKT8v8M0)
 [![Face Tracker](http://img.youtube.com/vi/WUOa3j-6XfI/0.jpg)](https://youtu.be/WUOa3j-6XfI)
+[![Color Pin Bot](http://img.youtube.com/vi/QY6nRYXQw_U/0.jpg)](https://youtu.be/QY6nRYXQw_U)
+[![BB-8 Joystick](http://img.youtube.com/vi/55kE9I4IQSU/0.jpg)](https://youtu.be/55kE9I4IQSU)
 
 
 ## Features
 
-- auto-detect and connect to Move Hub device
-- auto-detects peripheral devices connected to Hub
-- constant, angled and timed movement for motors, rotation sensor subscription
-- color & distance sensor: several modes to measure distance, color and luminosity
-- tilt sensor subscription: 2 axis, 3 axis, bump detect modes
-- LED color change
-- push button status subscription
-- battery voltage subscription available
+- auto-detect and connect to [Move Hub](docs/MoveHub.md) device
+- auto-detects [peripheral devices](docs/Peripherals.md) connected to Hub
+- constant, angled and timed movement for [motors](docs/Motor.md), rotation sensor subscription
+- [vision sensor](docs/VisionSensor.md): several modes to measure distance, color and luminosity
+- [tilt sensor](docs/TiltSensor.md) subscription: 2 axis, 3 axis, bump detect modes
+- [RGB LED](docs/LED.md) color change
+- [push button](docs/MoveHub.md#push-button) status subscription
+- [battery voltage and current](docs/VoltageCurrent.md) subscription available
 - permanent Bluetooth connection server for faster debugging
 
+
 ## Usage
 
 _Please note that this library requires one of Bluetooth backend libraries to be installed, please read section [here](#bluetooth-backend-prerequisites) for details._
 
 Install library like this: 
 ```bash
-pip install https://github.com/undera/pylgbst/archive/0.8.tar.gz
+pip install -U pylgbst
 ```
 
 Then instantiate MoveHub object and start invoking its methods. Following is example to just print peripherals detected on Hub:  
 
 ```python
-from pylgbst.movehub import MoveHub
+from pylgbst.hub import MoveHub
 
 hub = MoveHub()
 
-for device in hub.devices:
+for device in hub.peripherals:
     print(device)
 ```
 
-### Controlling Motors
+Each peripheral kind has own methods to do actions and/or get sensor data. See [features](#features) list for individual doc pages.
 
-MoveHub provides motors via following fields:
-- `motor_A` - port A
-- `motor_B` - port B
-- `motor_AB` - motor group of A+B manipulated together
-- `motor_external` - external motor attached to port C or D
+## Bluetooth Backend Prerequisites
 
-Methods to activate motors are:
-- `constant(speed_primary, speed_secondary)` - enables motor with specified speed forever 
-- `timed(time, speed_primary, speed_secondary)` - enables motor with specified speed for `time` seconds, float values accepted
-- `angled(angle, speed_primary, speed_secondary)` - makes motor to rotate to specified angle, `angle` value is integer degrees, can be negative and can be more than 360 for several rounds
-- `stop()` - stops motor at once, it is equivalent for `constant(0)`
-
-Parameter `speed_secondary` is used when it is motor group of `motor_AB` running together. By default, `speed_secondary` equals `speed_primary`. Speed values range is `-1.0` to `1.0`, float values. _Note: In group angled mode, total rotation angle is distributed across 2 motors according to motor speeds ratio._
-
-All these methods are synchronous by default, means method does not return untill it gets confirmation from Hub that command has completed. You can pass `async=True` parameter to any of methods to switch into asynchronous, which means command will return immediately, without waiting for rotation to complete. Be careful with asynchronous calls, as they make Hub to stop reporting synchronizing statuses.
-
-An example:
-```python
-from pylgbst.movehub import MoveHub
-import time
-
-hub = MoveHub()
-
-hub.motor_A.timed(0.5, 0.8)
-hub.motor_A.timed(0.5, -0.8)
-
-hub.motor_B.angled(90, 0.8)
-hub.motor_B.angled(-90, 0.8)
-
-hub.motor_AB.timed(1.5, 0.8, -0.8)
-hub.motor_AB.angled(90, 0.8, -0.8)
-
-hub.motor_external.constant(0.2)
-time.sleep(2)
-hub.motor_external.stop()
-```
-
-
-### Motor Rotation Sensors
-
-Any motor allows to subscribe to its rotation sensor. Two sensor modes are available: rotation angle (`EncodedMotor.SENSOR_ANGLE`) and rotation speed (`EncodedMotor.SENSOR_SPEED`). Example: 
-
-```python
-from pylgbst.movehub import MoveHub, EncodedMotor
-import time
-
-def callback(angle):
-    print("Angle: %s" % angle)
-
-hub = MoveHub()
-
-hub.motor_A.subscribe(callback, mode=EncodedMotor.SENSOR_ANGLE)
-time.sleep(60) # rotate motor A
-hub.motor_A.unsubscribe(callback)
-```
-
-### Tilt Sensor
-
-MoveHub's internal tilt sensor is available through `tilt_sensor` field. There are several modes to subscribe to sensor, providing 2-axis, 3-axis and bump detect data.
-
-An example:
-
-```python
-from pylgbst.movehub import MoveHub, TiltSensor
-import time
-
-def callback(pitch, roll, yaw):
-    print("Pitch: %s / Roll: %s / Yaw: %s" % (pitch, roll, yaw))
-
-hub = MoveHub()
-
-hub.tilt_sensor.subscribe(callback, mode=TiltSensor.MODE_3AXIS_FULL)
-time.sleep(60) # turn MoveHub block in different ways
-hub.tilt_sensor.unsubscribe(callback)
-```
-
-`TiltSensor` sensor mode constants:
-- `MODE_2AXIS_SIMPLE` - use `callback(state)` for 2-axis simple state detect
-- `MODE_2AXIS_FULL` - use `callback(roll, pitch)` for 2-axis roll&pitch degree values
-- `MODE_3AXIS_SIMPLE` - use `callback(state)` for 3-axis simple state detect
-- `MODE_3AXIS_FULL` - use `callback(roll, pitch)` for 2-axis roll&pitch degree values
-- `MODE_BUMP_COUNT` - use `callback(count)` to detect bumps
-
-There are tilt sensor constants for "simple" states, for 2-axis mode their names are also available through `TiltSensor.DUO_STATES`:
-- `DUO_HORIZ` - "HORIZONTAL"
-- `DUO_DOWN` - "DOWN"
-- `DUO_LEFT` - "LEFT"
-- `DUO_RIGHT` - "RIGHT"
-- `DUO_UP` - "UP"
-  
-For 3-axis simple mode map name is `TiltSensor.TRI_STATES` with values:
-- `TRI_BACK` - "BACK"
-- `TRI_UP` - "UP"
-- `TRI_DOWN` - "DOWN"
-- `TRI_LEFT` - "LEFT"
-- `TRI_RIGHT` - "RIGHT"
-- `TRI_FRONT` - "FRONT"
-
-
-### Color & Distance Sensor
-
-Field named `color_distance_sensor` holds instance of `ColorDistanceSensor`, if one is attached to MoveHub. Sensor has number of different modes to subscribe. 
-
-Colors that are detected are part of `COLORS` map (see [LED](#led) section). Only several colors are possible to detect: `BLACK`, `BLUE`, `CYAN`, `YELLOW`, `RED`, `WHITE`. Sensor does its best to detect best color, but only works when sample is very close to sensor.
-
-Distance works in range of 0-10 inches, with ability to measure last inch in higher detail.
-
-Simple example of subscribing to sensor:
-
-```python
-from pylgbst.movehub import MoveHub, ColorDistanceSensor
-import time
-
-def callback(clr, distance):
-    print("Color: %s / Distance: %s" % (clr, distance))
-
-hub = MoveHub()
-
-hub.color_distance_sensor.subscribe(callback, mode=ColorDistanceSensor.COLOR_DISTANCE_FLOAT)
-time.sleep(60) # play with sensor while it waits   
-hub.color_distance_sensor.unsubscribe(callback)
-```
-
-Subscription mode constants in class `ColorDistanceSensor` are:
-- `COLOR_DISTANCE_FLOAT` - default mode, use `callback(color, distance)` where `distance` is float value in inches
-- `COLOR_ONLY` - use `callback(color)`
-- `DISTANCE_INCHES` - use `callback(color)` measures distance in integer inches count
-- `COUNT_2INCH` - use `callback(count)` - it counts crossing distance ~2 inches in front of sensor
-- `DISTANCE_HOW_CLOSE` - use `callback(value)` - value of 0 to 255 for 30 inches, larger with closer distance
-- `DISTANCE_SUBINCH_HOW_CLOSE` - use `callback(value)` - value of 0 to 255 for 1 inch, larger with closer distance
-- `LUMINOSITY` - use `callback(luminosity)` where `luminosity` is float value from 0 to 1
-- `OFF1` and `OFF2` - seems to turn sensor LED and notifications off
-- `STREAM_3_VALUES` - use `callback(val1, val2, val3)`, sends some values correlating to distance, not well understood at the moment
-
-Tip: laser pointer pointing to sensor makes it to trigger distance sensor
-
-### LED
-
-`MoveHub` class has field `led` to access color LED near push button. To change its color, use `set_color(color)` method. 
-
-You can obtain colors are present as constants `COLOR_*` and also a map of available color-to-name as `COLORS`. There are 12 color values, including `COLOR_BLACK` and `COLOR_NONE` which turn LED off.
-
-Additionally, you can subscribe to LED color change events, using callback function as shown in example below.
-
-```python
-from pylgbst.movehub import MoveHub, COLORS, COLOR_NONE, COLOR_RED
-import time
-
-def callback(clr):
-    print("Color has changed: %s" % clr)
-
-hub = MoveHub()
-hub.led.subscribe(callback)
-
-hub.led.set_color(COLOR_RED)
-for color in COLORS:
-    hub.led.set_color(color)
-    time.sleep(0.5)
-    
-hub.led.set_color(COLOR_NONE)
-hub.led.unsubscribe(callback)
-```
-
-Tip: blinking orange color of LED means battery is low.
-
-### Push Button
-
-`MoveHub` class has field `button` to subscribe to button press and release events.
-
-Note that `Button` class is not real `Peripheral`, as it has no port and not listed in `devices` field of Hub. Still, subscribing to button is done usual way: 
-
-```python
-from pylgbst.movehub import MoveHub
-
-def callback(is_pressed):
-    print("Btn pressed: %s" % is_pressed)
-
-hub = MoveHub()
-hub.button.subscribe(callback)
-```
-
-### Power Voltage & Battery
-
-`MoveHub` class has field `voltage` to subscribe to battery voltage status. Callback accepts single parameter with current value. The range of values is float between `0` and `1.0`. Every time data is received, value is also written into `last_value` field of `Voltage` object. Values less than `0.2` are known as lowest values, when unit turns off.
-
-```python
-from pylgbst.movehub import MoveHub
-import time
-
-def callback(value):
-    print("Voltage: %s" % value)
-
-hub = MoveHub()
-hub.voltage.subscribe(callback)
-time.sleep(1)
-print ("Value: " % hub.voltage.last_value)
-```
-
-## General Notes
-
-### Bluetooth Backend Prerequisites
-
-You have following options to install as Bluetooth backend:
+You have following options to install as Bluetooth backend (some of them might require `sudo` on Linux):
 
 - `pip install pygatt` - [pygatt](https://github.com/peplin/pygatt) lib, works on both Windows and Linux  
 - `pip install gatt` - [gatt](https://github.com/getsenic/gatt-python) lib, supports Linux, does not work on Windows
 - `pip install gattlib` - [gattlib](https://bitbucket.org/OscarAcena/pygattlib) - supports Linux, does not work on Windows, requires `sudo`
+- `pip install bluepy` - [bluepy](https://github.com/IanHarvey/bluepy) lib, supports Linux, including Raspbian, which allows connection to the hub from the Raspberry PI
+- `pip install bleak` - [bleak](https://github.com/hbldh/bleak) lib, supports Linux/Windows/MacOS
 
 Running on Windows requires [Bluegiga BLED112 Bluetooth Smart Dongle](https://www.silabs.com/products/wireless/bluetooth/bluetooth-low-energy-modules/bled112-bluetooth-smart-dongle) hardware piece, because no other hardware currently works on Windows with Python+BLE.
 
@@ -265,54 +70,28 @@ _Please let author know if you have discovered any compatibility/preprequisite d
 Depending on backend type, you might need Linux `sudo` to be used when running Python.
 
 ### Bluetooth Connection Options
-There is optional parameter for `MoveHub` class constructor, accepting instance of `Connection` object. By default, it will try to use whatever `get_connection_auto()` returns. You have several options to manually control that:
+There is an optional parameter for `MoveHub` class constructor, accepting instance of `Connection` object. By default, it will try to use whatever `get_connection_auto()` returns. You have several options to manually control that:
 
-- use `pylgbst.get_connection_auto()` to attempt backend auto-choice, autodetect uses 
-- use `BlueGigaConnection()` - if you use BlueGiga Adapter (`pygatt` library prerequisite)
-- use `GattConnection()` - if you use Gatt Backend on Linux (`gatt` library prerequisite)
-- use `GattoolConnection()` - if you use GattTool Backend on Linux (`pygatt` library prerequisite)
-- use `GattLibConnection()` - if you use GattLib Backend on Linux (`gattlib` library prerequisite)
+- use `get_connection_auto()` to attempt backend auto-detection 
+- use `get_connection_bluegiga()` - if you use BlueGiga Adapter (`pygatt` library prerequisite)
+- use `get_connection_gatt()` - if you use Gatt Backend on Linux (`gatt` library prerequisite)
+- use `get_connection_gattool()` - if you use GattTool Backend on Linux (`pygatt` library prerequisite)
+- use `get_connection_gattlib()` - if you use GattLib Backend on Linux (`gattlib` library prerequisite)
+- use `get_connection_bluepy()` - if you use Bluepy backend on Linux/Raspbian (`bluepy` library prerequisite)
+- use `get_connection_bleak()` - if you use Bleak backend (`bleak` library prerequisite)
 - pass instance of `DebugServerConnection` if you are using [Debug Server](#debug-server) (more details below).
 
-All the functions above have optional arguments to specify adapter name and MoveHub mac address. Please look function source code for details.
+All the functions above have optional arguments to specify adapter name and Hub name (or mac address). Please take a look at functions source code for details.
 
-If you want to specify name for Bluetooth interface to use on local computer, you can passthat to class or function of getting a connection. Then pass connection object to `MoveHub` constructor. Like this:
+If you want to specify name for Bluetooth interface to use on local computer, you can pass that to class or function of getting a connection. Then pass connection object to `MoveHub` constructor. Like this:
 ```python
-from pylgbst.movehub import MoveHub
-from pylgbst.comms.cgatt import GattConnection
-
-conn = GattConnection("hci1")
-conn.connect()  # you can pass MoveHub mac address as parameter here, like 'AA:BB:CC:DD:EE:FF'
+from pylgbst.hub import MoveHub
+from pylgbst import get_connection_gatt
 
+conn = get_connection_gatt(hub_mac='AA:BB:CC:DD:EE:FF')
 hub = MoveHub(conn)
 ```
 
-### Use Disconnect in `finally`
-
-It is recommended to make sure `disconnect()` method is called on connection object after you have finished your program. This ensures Bluetooth subsystem is cleared and avoids problems for subsequent re-connects of MoveHub. The best way to do that in Python is to use `try ... finally` clause:
-
-```python
-from pylgbst import get_connection_auto
-from pylgbst.movehub import MoveHub
-
-conn=get_connection_auto()  # ! don't put this into `try` block
-try:
-    hub = MoveHub(conn)
-finally:
-    conn.disconnect()
-```
-
-### Devices Detecting
-As part of instantiating process, `MoveHub` waits up to 1 minute for all builtin devices to appear, such as motors on ports A and B, tilt sensor, button and battery. This not guarantees that external motor and/or color sensor will be present right after `MoveHub` instantiated. Usually, sleeping for couple of seconds gives it enough time to detect everything.
-
-### Subscribing to Sensors
-Each sensor usually has several different "subscription modes", differing with callback parameters and value semantics. 
-
-There is optional `granularity` parameter for each subscription call, by default it is `1`. This parameter tells Hub when to issue sensor data notification. Value of notification has to change greater or equals to `granularity` to issue notification. This means that specifying `0` will cause it to constantly send notifications, and specifying `5` will cause less frequent notifications, only when values change for more than `5` (inclusive).
-
-It is possible to subscribe with multiple times for the same sensor. Only one, very last subscribe mode is in effect, with many subscriber callbacks allowed to receive notifications. 
-
-Good practice for any program is to unsubscribe from all sensor subscriptions before ending, especially when used with `DebugServer`.
 
 ## Debug Server
 Running debug server opens permanent BLE connection to Hub and listening on TCP port for communications. This avoids the need to re-start Hub all the time. 
@@ -329,17 +108,15 @@ Then push green button on MoveHub, so permanent BLE connection will be establish
 
 ## Roadmap & TODO
 
+- validate operations with other Hub types (train, PUP etc)
+- make connections to detect hub by UUID instead of name
 - document all API methods
-- make sure unit tests cover all important code
 - make debug server to re-establish BLE connection on loss
 
 ## Links
 
-- https://github.com/JorgePe/BOOSTreveng - source of protocol knowledge
+- https://github.com/LEGO/lego-ble-wireless-protocol-docs - true docs for LEGO BLE protocol
+- https://github.com/JorgePe/BOOSTreveng - initial source of protocol knowledge
+- https://github.com/nathankellenicki/node-poweredup - JavaScript version of library
 - https://github.com/spezifisch/sphero-python/blob/master/BB8joyDrive.py - example with another approach to bluetooth libs
-
-Some things around visual programming:
-- https://github.com/RealTimeWeb/blockpy
-- https://ru.wikipedia.org/wiki/App_Inventor
-- https://en.wikipedia.org/wiki/Blockly
-
+- https://github.com/virantha/bricknil - for the lovers of async Python, alternative implementation of library to control PoweredUp Hubs

docs/GenericHub.md

@@ -0,0 +1,26 @@
+# Generic Powered Up Hub
+
+## Connecting to Hub via Bluetooth
+
+## Accessing Peripherals
+
+## Sending and Receiving Low-Level Messages
+`Hub.send(msg)`
+add_message_handler
+
+## Use Disconnect in `finally`
+
+It is recommended to make sure `disconnect()` method is called on connection object after you have finished your program. This ensures Bluetooth subsystem is cleared and avoids problems for subsequent re-connects of MoveHub. The best way to do that in Python is to use `try ... finally` clause:
+
+```python
+from pylgbst import get_connection_auto
+from pylgbst.hub import Hub
+
+conn = get_connection_auto()  # ! don't put this into `try` block
+try:
+    hub = Hub(conn)
+finally:
+    conn.disconnect()
+```
+
+Additionally, hub has `Hub.disconnect()` and `Hub.switch_off()` methods to call corresponding commands.

docs/LED.md

@@ -0,0 +1,31 @@
+### LED
+
+`MoveHub` class has field `led` to access color LED near push button. To change its color, use `set_color(color)` method. 
+
+You can obtain colors are present as constants `COLOR_*` and also a map of available color-to-name as `COLORS`. There are 12 color values, including `COLOR_BLACK` and `COLOR_NONE` which turn LED off.
+
+Additionally, you can subscribe to LED color change events, using callback function as shown in example below.
+
+```python
+from pylgbst.hub import MoveHub, COLORS, COLOR_NONE, COLOR_RED
+import time
+
+def callback(clr):
+    print("Color has changed: %s" % clr)
+
+hub = MoveHub()
+hub.led.subscribe(callback)
+
+hub.led.set_color(COLOR_RED)
+for color in COLORS:
+    hub.led.set_color(color)
+    time.sleep(0.5)
+    
+hub.led.set_color(COLOR_NONE)
+hub.led.unsubscribe(callback)
+```
+
+Tip: blinking orange color of LED means battery is low.
+
+
+Note that Vision Sensor can also be used to set its LED color into indexed colors.

docs/Motor.md

@@ -0,0 +1,55 @@
+# Motors
+
+![](https://img.bricklink.com/ItemImage/PL/6181852.png)
+
+## Controlling Motors
+
+Methods to activate motors are:
+- `start_speed(speed_primary, speed_secondary)` - enables motor with specified speed forever 
+- `timed(time, speed_primary, speed_secondary)` - enables motor with specified speed for `time` seconds, float values accepted
+- `angled(angle, speed_primary, speed_secondary)` - makes motor to rotate to specified angle, `angle` value is integer degrees, can be negative and can be more than 360 for several rounds
+- `stop()` - stops motor
+
+Parameter `speed_secondary` is used when it is motor group of `motor_AB` running together. By default, `speed_secondary` equals `speed_primary`. 
+
+Speed values range is `-1.0` to `1.0`, float values. _Note: In group angled mode, total rotation angle is distributed across 2 motors according to motor speeds ratio, see official doc [here](https://lego.github.io/lego-ble-wireless-protocol-docs/index.html#tacho-math)._
+
+An example:
+```python
+from pylgbst.hub import MoveHub
+import time
+
+hub = MoveHub()
+
+hub.motor_A.timed(0.5, 0.8)
+hub.motor_A.timed(0.5, -0.8)
+
+hub.motor_B.angled(90, 0.8)
+hub.motor_B.angled(-90, 0.8)
+
+hub.motor_AB.timed(1.5, 0.8, -0.8)
+hub.motor_AB.angled(90, 0.8, -0.8)
+
+hub.motor_external.start_speed(0.2)
+time.sleep(2)
+hub.motor_external.stop()
+```
+
+
+## Motor Rotation Sensors
+
+Any motor allows to subscribe to its rotation sensor. Two sensor modes are available: rotation angle (`EncodedMotor.SENSOR_ANGLE`) and rotation speed (`EncodedMotor.SENSOR_SPEED`). Example: 
+
+```python
+from pylgbst.hub import MoveHub, EncodedMotor
+import time
+
+def callback(angle):
+    print("Angle: %s" % angle)
+
+hub = MoveHub()
+
+hub.motor_A.subscribe(callback, mode=EncodedMotor.SENSOR_ANGLE)
+time.sleep(60) # rotate motor A
+hub.motor_A.unsubscribe(callback)
+```

docs/MoveHub.md

@@ -0,0 +1,43 @@
+# Move Hub
+
+![](http://bricker.info/images/parts/26910c01.png)
+
+`MoveHub` is extension of generic [Powered Up Hub](GenericHub.md) class. `MoveHub` class delivers specifics of MoveHub brick, such as internal motor port names. Apart from specifics listed below, all operations on Hub are done [as usual](GenericHub.md).
+
+## Devices Detecting
+As part of instantiating process, `MoveHub` waits up to 1 minute for builtin devices to appear, such as motors on ports A and B, [tilt sensor](TiltSensor.md), [LED](LED.md) and [battery](VoltageCurrent.md). This not guarantees that external motor and/or color sensor will be present right after `MoveHub` instantiated. Usually, `time.sleep(1.0)` for couple of seconds gives it enough time to detect everything.
+
+MoveHub provides motors via following fields:
+- `motor_A` - port A motor
+- `motor_B` - port B motor
+- `motor_AB` - combined motors A+B manipulated together
+- `motor_external` - external motor attached to port C or D
+
+MoveHub's internal [tilt sensor](TiltSensor.md) is available through `tilt_sensor` field. 
+
+Field named `vision_sensor` holds instance of [`VisionSensor`](VisionSensor.md), if one is attached to MoveHub.
+
+Fields named `current` and `voltage` present [corresponding sensors](VoltageCurrent.md) from Hub.
+
+## Push Button
+
+`MoveHub` class has field `button` to subscribe to button press and release events.
+
+Note that `Button` class is not real `Peripheral`, as it has no port and not listed in `peripherals` field of Hub. For convenience, subscribing to button is still done usual way: 
+
+```python
+from pylgbst.hub import MoveHub
+
+def callback(is_pressed):
+    print("Btn pressed: %s" % is_pressed)
+
+hub = MoveHub()
+hub.button.subscribe(callback)
+```
+
+The state for button has 3 possible values: 
+- `0` - not released
+- `1` - pressed
+- `2` - pressed
+
+It is for now unknown why Hub always issues notification with `1` and immediately with `2`, after button is pressed.

docs/Peripherals.md

@@ -0,0 +1,24 @@
+# Peripheral Types
+
+Here is the list of peripheral devices that have dedicated classes in library:
+
+- [Motors](Motor.md)
+- [RGB LED](LED.md)
+- [Tilt Sensor](TiltSensor.md)
+- [Vision Sensor](VisionSensor.md) (color and/or distance)
+- [Voltage and Current Sensors](VoltageCurrent.md)
+
+In case device you attached to Hub is of an unknown type, it will get generic `Peripheral` class, allowing direct low-level interactions.
+
+## Subscribing to Sensors
+Each sensor usually has several different "subscription modes", differing with callback parameters and value semantics. 
+
+There is optional `granularity` parameter for each subscription call, by default it is `1`. This parameter tells Hub when to issue sensor data notification. Value of notification has to change greater or equals to `granularity` to issue notification. This means that specifying `0` will cause it to constantly send notifications, and specifying `5` will cause less frequent notifications, only when values change for more than `5` (inclusive).
+
+It is possible to subscribe with multiple times for the same sensor. Only one, very last subscribe mode is in effect, with many subscriber callbacks allowed to receive notifications. 
+
+Good practice for any program is to unsubscribe from all sensor subscriptions before exiting, especially when used with `DebugServer`.
+
+## Generic Perihpheral 
+
+In case you have used a peripheral that is not recognized by the library, it will be detected as generic `Peripheral` class. You still can use subscription and sensor info getting commands for it.  

docs/TiltSensor.md

@@ -0,0 +1,42 @@
+### Tilt Sensor
+
+There are several modes to subscribe to sensor, providing 2-axis, 3-axis and bump detect data.
+
+An example:
+
+```python
+from pylgbst.hub import MoveHub, TiltSensor
+import time
+
+def callback(pitch, roll, yaw):
+    print("Pitch: %s / Roll: %s / Yaw: %s" % (pitch, roll, yaw))
+
+hub = MoveHub()
+
+hub.tilt_sensor.subscribe(callback, mode=TiltSensor.MODE_3AXIS_SIMPLE)
+time.sleep(60) # turn MoveHub block in different ways
+hub.tilt_sensor.unsubscribe(callback)
+```
+
+`TiltSensor` sensor mode constants:
+- `MODE_2AXIS_SIMPLE` - use `callback(state)` for 2-axis simple state detect
+- `MODE_2AXIS_FULL` - use `callback(roll, pitch)` for 2-axis roll&pitch degree values
+- `MODE_3AXIS_SIMPLE` - use `callback(state)` for 3-axis simple state detect
+- `MODE_3AXIS_FULL` - use `callback(roll, pitch)` for 2-axis roll&pitch degree values
+- `MODE_BUMP_COUNT` - use `callback(count)` to detect bumps
+
+There are tilt sensor constants for "simple" states, for 2-axis mode their names are also available through `TiltSensor.DUO_STATES`:
+- `DUO_HORIZ` - "HORIZONTAL"
+- `DUO_DOWN` - "DOWN"
+- `DUO_LEFT` - "LEFT"
+- `DUO_RIGHT` - "RIGHT"
+- `DUO_UP` - "UP"
+  
+For 3-axis simple mode map name is `TiltSensor.TRI_STATES` with values:
+- `TRI_BACK` - "BACK"
+- `TRI_UP` - "UP"
+- `TRI_DOWN` - "DOWN"
+- `TRI_LEFT` - "LEFT"
+- `TRI_RIGHT` - "RIGHT"
+- `TRI_FRONT` - "FRONT"
+

docs/VisionSensor.md

@@ -0,0 +1,36 @@
+### Color & Distance Sensor
+![](https://img.bricklink.com/ItemImage/PL/6182145.png)
+
+ Sensor has number of different modes to subscribe. 
+
+Colors that are detected are part of `COLORS` map (see [LED](#led) section). Only several colors are possible to detect: `BLACK`, `BLUE`, `CYAN`, `YELLOW`, `RED`, `WHITE`. Sensor does its best to detect best color, but only works when sample is very close to sensor.
+
+Distance works in range of 0-10 inches, with ability to measure last inch in higher detail.
+
+Simple example of subscribing to sensor:
+
+```python
+from pylgbst.hub import MoveHub, VisionSensor
+import time
+
+def callback(clr, distance):
+    print("Color: %s / Distance: %s" % (clr, distance))
+
+hub = MoveHub()
+
+hub.vision_sensor.subscribe(callback, mode=VisionSensor.COLOR_DISTANCE_FLOAT)
+time.sleep(60) # play with sensor while it waits   
+hub.vision_sensor.unsubscribe(callback)
+```
+
+Subscription mode constants in class `ColorDistanceSensor` are:
+- `COLOR_DISTANCE_FLOAT` - default mode, use `callback(color, distance)` where `distance` is float value in inches
+- `COLOR_ONLY` - use `callback(color)`
+- `DISTANCE_INCHES` - use `callback(color)` measures distance in integer inches count
+- `COUNT_2INCH` - use `callback(count)` - it counts crossing distance ~2 inches in front of sensor
+- `DISTANCE_HOW_CLOSE` - use `callback(value)` - value of 0 to 255 for 30 inches, larger with closer distance
+- `DISTANCE_SUBINCH_HOW_CLOSE` - use `callback(value)` - value of 0 to 255 for 1 inch, larger with closer distance
+- `LUMINOSITY` - use `callback(luminosity)` where `luminosity` is float value from 0 to 1
+- `OFF1` and `OFF2` - seems to turn sensor LED and notifications off
+- `STREAM_3_VALUES` - use `callback(val1, val2, val3)`, sends some values correlating to distance, not well understood at the moment
+

docs/VoltageCurrent.md

@@ -0,0 +1,14 @@
+### Power Voltage & Battery
+
+`MoveHub` class has field `voltage` to subscribe to battery voltage status. Callback accepts single parameter with current value. The range of values is float between `0` and `1.0`. Every time data is received, value is also written into `last_value` field of `Voltage` object. Values less than `0.2` are known as lowest values, when unit turns off.
+
+```python
+from pylgbst.hub import MoveHub, Voltage
+
+def callback(value):
+    print("Voltage: %s" % value)
+
+hub = MoveHub()
+print ("Value L: " % hub.voltage.get_sensor_data(Voltage.VOLTAGE_L))
+print ("Value S: " % hub.voltage.get_sensor_data(Voltage.VOLTAGE_S))
+```

examples/advancedbutton/README.md

@@ -0,0 +1,40 @@
+### Advanced button
+
+This example shows how you can add additional functionallity to the move hub button. 
+
+It adds three new actions which you can use instead of the standard subscription to a button press:
+
+- Click - a single quick up/down press
+- Double click - a double up/down press, second click must occur within .5 secs of first one 
+- Long press - a press and hold on the button for > .7 secs 
+
+```python
+from pylgbst.hub import MoveHub
+from advancedbutton import AdvancedButton
+import time
+
+
+hub = MoveHub()
+b = AdvancedButton(hub)
+
+
+def clicked():
+    print("button clicked")
+
+
+def pressed():
+    print("button pressed")
+
+
+def doubleclicked():
+    print("button double clicked")
+
+
+b.click.subscribe(clicked)
+b.double_click.subscribe(doubleclicked)
+b.long_press.subscribe(pressed)
+
+time.sleep(120)
+```
+
+You can alter the timings using the two constants `DOUBLE_CLICK_TIME` and `LONG_PRESS_TIME`

examples/advancedbutton/advancedbutton.py

@@ -0,0 +1,73 @@
+import time
+import threading
+
+DOUBLE_CLICK_TIME = 0.5
+LONG_PRESS_TIME = 0.7
+
+
+class AdvancedButton:
+    def __init__(self, hub):
+        self.state = 0
+        self.last_pressed = 0
+        self.press_time = None
+        self.hub = hub
+        self.hub.button.subscribe(self.pressed)
+        self.click = ButtonAction()
+        self.double_click = ButtonAction()
+        self.long_press = ButtonAction()
+
+    def pressed(self, state):
+        if state == 2:
+            return
+
+        press_time = time.time()
+
+        if state == 1:
+            self.state = 1
+            self.press_time = press_time
+            return
+
+        if state == 0 and self.state == 1:
+            self.state = 0
+            press_duration = press_time - self.press_time
+        else:
+            return
+
+        if press_duration > LONG_PRESS_TIME:
+            # long press
+            self.long_press.notify()
+            return
+
+        if (press_time - self.last_pressed) < DOUBLE_CLICK_TIME:
+            # double click
+            self.last_pressed = 0
+            self.double_click.notify()
+            return
+
+        # could be first of a double click, could be single click
+        self.last_pressed = press_time
+
+        def timeout():
+            time.sleep(DOUBLE_CLICK_TIME)
+            if self.last_pressed == press_time:
+                # not clicked while sleeping
+                # single click
+                self.click.notify()
+
+        threading.Thread(target=timeout).start()
+
+
+class ButtonAction:
+    def __init__(self):
+        self.subscribers = set()
+
+    def subscribe(self, callback):
+        self.subscribers.add(callback)
+
+    def unsubscribe(self, callback=None):
+        if callback in self.subscribers:
+            self.subscribers.remove(callback)
+
+    def notify(self):
+        for subscriber in self.subscribers.copy():
+            subscriber()

examples/automata/__init__.py

@@ -0,0 +1,79 @@
+import logging
+import time
+from collections import Counter
+
+from pylgbst.hub import MoveHub, COLOR_NONE, COLOR_BLACK, COLORS, COLOR_CYAN, COLOR_BLUE, COLOR_RED, COLOR_YELLOW, \
+    COLOR_WHITE
+from pylgbst.peripherals import EncodedMotor
+
+
+class Automata(object):
+    BASE_SPEED = 1.0
+
+    def __init__(self):
+        super(Automata, self).__init__()
+        self.__hub = MoveHub()
+        self.__hub.vision_sensor.subscribe(self.__on_sensor)
+        self._sensor = []
+
+    def __on_sensor(self, color, distance=-1):
+        logging.debug("Sensor data: %s/%s", COLORS[color], distance)
+        if distance <= 4:
+            if color not in (COLOR_NONE, COLOR_BLACK):
+                self._sensor.append((color, int(distance)))
+                logging.debug("Sensor data: %s", COLORS[color])
+
+    def feed_tape(self):
+        self.__hub.motor_external.angled(60, 0.5)
+        time.sleep(0.1)
+        self.__hub.motor_external.angled(60, 0.5)
+        time.sleep(0.1)
+
+    def get_color(self):
+        res = self._sensor
+        self._sensor = []
+        logging.debug("Sensor data: %s", res)
+        cnts = Counter([x[0] for x in res])
+        clr = cnts.most_common(1)[0][0] if cnts else COLOR_NONE
+        if clr == COLOR_CYAN:
+            clr = COLOR_BLUE
+        self.__hub.led.set_color(clr)
+        return clr
+
+    def left(self):
+        self.__hub.motor_AB.angled(290, self.BASE_SPEED, -self.BASE_SPEED, end_state=EncodedMotor.END_STATE_FLOAT)
+        time.sleep(0.1)
+        self.__hub.motor_AB.stop()
+
+    def right(self):
+        self.__hub.motor_AB.angled(270, -self.BASE_SPEED, self.BASE_SPEED, end_state=EncodedMotor.END_STATE_FLOAT)
+        time.sleep(0.1)
+        self.__hub.motor_AB.stop()
+
+    def forward(self):
+        self.__hub.motor_AB.angled(500, self.BASE_SPEED)
+
+    def backward(self):
+        self.__hub.motor_AB.angled(500, -self.BASE_SPEED)
+
+
+if __name__ == '__main__':
+    logging.basicConfig(level=logging.INFO)
+    bot = Automata()
+
+    color = None
+    cmds = []
+    while color != COLOR_NONE:
+        bot.feed_tape()
+        color = bot.get_color()
+
+        logging.warning(COLORS[color])
+
+        if color == COLOR_BLUE:
+            bot.forward()
+        elif color == COLOR_RED:
+            bot.backward()
+        elif color == COLOR_YELLOW:
+            bot.left()
+        elif color == COLOR_WHITE:
+            bot.right()

examples/automata/bot.py

@@ -0,0 +1,34 @@
+from examples.automata import Automata
+
+from pylgbst.peripherals import *
+
+
+# this program is written by Sofia in early 2019
+
+def action_by_color(color):
+    if color == COLOR_BLUE:
+        bot.forward()
+    if color == COLOR_RED:
+        bot.backward()
+    if color == COLOR_WHITE:
+        bot.right()
+    if color == COLOR_YELLOW:
+        bot.left()
+
+
+def read_color():
+    bot.feed_tape()
+    color = bot.get_color()
+    print(COLORS[color])
+    return color
+
+
+bot = Automata()
+number = 0
+color = None
+while color != COLOR_NONE:
+    color = read_color()
+    number = number + 1
+    action_by_color(color)
+
+print(number)

examples/bb8joystick/__init__.py

@@ -0,0 +1,57 @@
+import logging
+import sys
+import time
+
+from examples.bb8joystick import joystick
+from examples.bb8joystick.bb8 import BB8
+from examples.bb8joystick.joystick import Joystick
+
+if __name__ == "__main__":
+    logging.basicConfig(level=logging.DEBUG if 'pydevd' in sys.modules else logging.WARNING)
+
+    bb8 = BB8()
+    joystick = Joystick()
+
+
+    def set_bb_color(flag):
+        if flag:
+            bb8.color(255, 255, 255)
+        else:
+            bb8.color(0, 0, 0)
+
+
+    def set_heading(angle):
+        a = int(angle) % 360
+        print("Angle", a)
+        bb8.heading(a)
+
+
+    def roll(speed, direction):
+        print("Roll", speed, direction)
+        if speed < 3:
+            speed = 0
+        bb8.roll(speed, direction)
+
+
+    def stop(state):
+        if state:
+            print("Stop")
+            bb8.roll(0, 0)
+        else:
+            print("Stabilize")
+            bb8.color(255, 0, 255)
+            bb8.stabilize()
+            bb8.color(0, 0, 0)
+
+
+    try:
+        joystick.on_button(set_bb_color)
+        joystick.on_button(stop)
+        joystick.on_rotation(set_heading)
+        joystick.on_joystick(roll)
+        print("All set up")
+
+        time.sleep(300)
+    finally:
+        bb8.disconnect()
+        joystick.disconnect()

examples/bb8joystick/bb8.py

@@ -0,0 +1,113 @@
+import asyncio
+import time
+
+import spheropy
+# noinspection PyProtectedMember
+from spheropy.spheropy import _ClientCommandPacket, _DEVICE_ID_CORE, _DEVICE_ID_SPHERO
+
+
+class BLEInterfaceGattool(spheropy.BleInterface):
+    def _find_adapter(self):
+        adapter = spheropy.pygatt.GATTToolBackend()
+        adapter.start()
+        adapter_type = spheropy.BleInterface.BleAdapterType.PYGATT
+
+        self._adapter = adapter
+        self._adapter_type = adapter_type
+
+        return True
+
+
+class _SpheroImproved(spheropy.Sphero):
+    async def connect(self, search_name=None, address=None, port=None, bluetooth_interface=None, use_ble=False,
+                      num_retry_attempts=1):
+        gattool = BLEInterfaceGattool(search_name)
+        return await
+        super().connect(search_name, address, port, gattool, use_ble, num_retry_attempts)
+
+    async def sleep(self, sleeptime, reset_inactivity_timeout=True, response_timeout_in_seconds=None):
+        # port from https://github.com/jchadwhite/SpheroBB8-python/blob/master/BB8_driver.py#L394
+        command = _ClientCommandPacket(device_id=_DEVICE_ID_CORE,
+                                       command_id=0x22,
+                                       sequence_number=self._get_and_increment_command_sequence_number(),
+                                       data=[(sleeptime >> 8), (sleeptime & 0xff), 0],
+                                       wait_for_response=False,
+                                       reset_inactivity_timeout=reset_inactivity_timeout)
+
+        return await
+        self._send_command(command, response_timeout_in_seconds)
+
+    async def set_rotation_rate(self, rate, reset_inactivity_timeout=True, response_timeout_in_seconds=None):
+        # port from https://github.com/jchadwhite/SpheroBB8-python/blob/master/BB8_driver.py
+        command = _ClientCommandPacket(device_id=_DEVICE_ID_SPHERO,
+                                       command_id=0x03,
+                                       sequence_number=self._get_and_increment_command_sequence_number(),
+                                       data=[rate],
+                                       wait_for_response=False,
+                                       reset_inactivity_timeout=reset_inactivity_timeout)
+
+        return await
+        self._send_command(command, response_timeout_in_seconds)
+
+
+class BB8(object):
+    def __init__(self, name="BB-CC13"):
+        self._heading = 0
+        # marry sync with async https://www.aeracode.org/2018/02/19/python-async-simplified/
+        self._loop = asyncio.new_event_loop()
+        asyncio.set_event_loop(self._loop)
+
+        print("Started to wake up BB-8...")
+        self._sphero = _SpheroImproved()
+        self._loop.run_until_complete(self._sphero.connect(num_retry_attempts=3, use_ble=True, search_name=name))
+        self._loop.run_until_complete(self._sphero.set_stabilization(True))
+        self._loop.run_until_complete(self._sphero.set_rotation_rate(1))
+        self.color(0, 0xFF, 0)
+        self.stabilize()
+        print("BB-8 is ready for commands")
+
+    def disconnect(self):
+        print("BB8 enters sleep")
+        self._loop.run_until_complete(self._sphero.sleep(0))
+        self._sphero.disconnect()
+
+    def color(self, red, green, blue):
+        self._wait_loop()
+        self._loop.run_until_complete(self._sphero.set_rgb_led(red, green, blue, wait_for_response=False))
+
+    def heading(self, heading):
+        self._wait_loop()
+        heading = 359 - heading
+        self._heading = heading
+        # self._loop.run_until_complete(self._sphero.set_heading(359 - heading))
+        self._loop.run_until_complete(self._sphero.roll(1, heading, spheropy.RollMode.IN_PLACE_ROTATE))
+
+    def roll(self, speed=10, direction=0):
+        self._wait_loop()
+        direction += self._heading
+        direction %= 360
+        speed = int(255 * speed / 10)
+        speed *= 0.75  # throttle down a bit
+        self._loop.run_until_complete(self._sphero.roll(int(speed), direction))
+
+    def stop(self):
+        self._wait_loop()
+        self._loop.run_until_complete(self._sphero.roll(0, 0))
+
+    def stabilize(self):
+        self._wait_loop()
+        self._loop.run_until_complete(self._sphero.self_level())
+
+    def _wait_loop(self):
+        while self._loop.is_running():
+            time.sleep(0.001)
+
+
+if __name__ == '__main__':
+    bb8 = BB8()
+    bb8.color(255, 0, 0)
+    time.sleep(1)
+    bb8.color(0, 255, 0)
+    time.sleep(1)
+    bb8.color(0, 0, 255)
+    time.sleep(1)

examples/bb8joystick/joystick.py

@@ -0,0 +1,145 @@
+import logging
+import math
+import sys
+import time
+
+from pylgbst.hub import MoveHub
+
+
+def _clamp(minvalue, value, maxvalue):
+    return max(minvalue, min(value, maxvalue))
+
+
+class Joystick(object):
+    RANGE_A = 40
+    RANGE_C = 30
+
+    def __init__(self):
+        super(Joystick, self).__init__()
+        self._on_joystick = set()
+        self.button_pressed = False
+        self._angle_A = 0
+        self.angle_B = 0
+        self._angle_C = 0
+
+        print("Starting search for Joystick...")
+        self._hub = MoveHub()
+        self._reset_sensors()
+        self._hub.button.subscribe(self._on_btn)
+        self._on_motor_a(self._on_a)
+        self.on_rotation(self._on_b)
+        self._on_motor_c(self._on_c)
+
+        print("Joystick is ready")
+
+    def disconnect(self):
+        print("Joystick disconnects")
+        self._hub.disconnect()
+
+    def _reset_sensors(self):
+        logging.info("Resetting motor encoders")
+        self._hub.motor_A.preset_encoder()
+        self._hub.motor_B.preset_encoder()
+        self._hub.motor_external.preset_encoder()
+
+    def on_button(self, callback):
+        """
+        Notifies about button state change. ``callback(state)`` gets single bool parameter
+        """
+
+        def wrapper(state):
+            if state in (0, 1):
+                callback(bool(state))
+
+        self._hub.button.subscribe(wrapper)
+
+    def _on_motor_a(self, callback):
+        def wrapper(angle):
+            logging.debug("Raw angle: %s", angle)
+            angle = _clamp(-self.RANGE_A, angle, self.RANGE_A)
+            callback(angle)
+
+        self._hub.motor_A.subscribe(wrapper)
+
+    def on_rotation(self, callback):
+        """
+        Notifies about B motor rotation. ``callback(state)`` gets single int parameter from 0 to 359
+        """
+
+        def wrapper(angle):
+            logging.debug("Raw angle: %s", angle)
+            val = angle % 360
+            val = val if val >= 0 else 360 - val - 1
+            val = 359 - val
+            callback(val)
+
+        self._hub.motor_B.subscribe(wrapper)
+
+    def _on_motor_c(self, callback):
+        def wrapper(angle):
+            logging.debug("Raw angle: %s", angle)
+            angle = _clamp(-self.RANGE_C, angle, self.RANGE_C)
+            callback(angle)
+
+        self._hub.motor_external.subscribe(wrapper)
+
+    def _on_btn(self, state):
+        self.button_pressed = bool(state)
+
+    def _on_a(self, angle):
+        logging.debug("A rotated: %s", angle)
+        self._angle_A = angle
+        self._calc_joystick()
+
+    def _on_b(self, angle):
+        logging.debug("B rotated: %s", angle)
+        self.angle_B = angle
+
+    def _on_c(self, angle):
+        logging.debug("C rotated: %s", angle)
+        self._angle_C = angle
+        self._calc_joystick()
+
+    def on_joystick(self, callback):
+        """
+        Notifies about joystick change. ``callback(speed, direction)`` gets parameters:
+        - ``speed`` - int value from 0 to 10
+        - ``direction`` - int value from 0 to 359
+
+        """
+        self._on_joystick.add(callback)
+
+    def _calc_joystick(self):
+        norm_a = self._angle_A / self.RANGE_A
+        norm_b = self._angle_C / self.RANGE_C
+        logging.debug("%s / %s", self._angle_A, self._angle_C)
+        logging.debug("%s / %s", norm_a, norm_b)
+        speed = math.sqrt(norm_a ** 2 + norm_b ** 2)  # / math.sqrt(2)
+        speed = _clamp(-1.0, speed, 1.0)
+
+        maxsize = sys.maxsize if norm_a >= 0 else -sys.maxsize
+        direction = math.atan(norm_a / norm_b if norm_b else maxsize)
+        direction *= 180 / math.pi
+        if norm_a >= 0 and norm_b >= 0:
+            direction = 90 - direction
+        elif norm_a < 0 and norm_b >= 0:
+            direction = 90 - direction
+        elif norm_a < 0 and norm_b < 0:
+            direction = 270 - direction
+        else:
+            direction = 270 - direction
+
+        for callback in self._on_joystick:
+            callback(int(round(10 * speed)), int(direction))
+
+
+if __name__ == '__main__':
+    logging.basicConfig(level=logging.INFO)
+
+    stick = Joystick()
+
+    stick.on_button(lambda x: logging.info("Button: %s" % x))
+    stick.on_rotation(lambda x: logging.info("Rotation: %s" % x))
+    stick.on_joystick(lambda speed, head: logging.info("Speed: %s, Direction: %s" % (speed, head)))
+
+    time.sleep(100)

examples/bb8joystick/program.py

@@ -0,0 +1,18 @@
+import time
+
+from examples.bb8joystick import BB8, Joystick
+
+bb8 = BB8()
+
+
+def button(p):
+    print(p)
+    if p == True:
+        bb8.color(255, 255, 255)
+    else:
+        bb8.color(0, 0, 0)
+
+
+deck = Joystick()
+deck.on_button(button)
+time.sleep(60)

examples/demo.py

@@ -3,9 +3,8 @@ import time
 from time import sleep
 
 from pylgbst import *
-from pylgbst.comms import DebugServerConnection
-from pylgbst.movehub import MoveHub, COLORS, COLOR_BLACK
-from pylgbst.peripherals import EncodedMotor, TiltSensor, Amperage, Voltage
+from pylgbst.hub import MoveHub
+from pylgbst.peripherals import EncodedMotor, TiltSensor, Current, Voltage, COLORS, COLOR_BLACK
 
 log = logging.getLogger("demo")
 
@@ -13,8 +12,13 @@ log = logging.getLogger("demo")
 def demo_led_colors(movehub):
     # LED colors demo
     log.info("LED colors demo")
-    movehub.color_distance_sensor.subscribe(lambda x, y: None)
-    for color in COLORS.keys()[1:] + [COLOR_BLACK]:
+
+    # We get a response with payload and port, not x and y here...
+    def colour_callback(named):
+        log.info("LED Color callback: %s", named)
+
+    movehub.led.subscribe(colour_callback)
+    for color in list(COLORS.keys())[1:] + [COLOR_BLACK]:
         log.info("Setting LED color to: %s", COLORS[color])
         movehub.led.set_color(color)
         sleep(1)
@@ -95,7 +99,7 @@ def demo_tilt_sensor_precise(movehub):
         demo_tilt_sensor_simple.cnt += 1
         log.info("Tilt #%s of %s: roll:%s pitch:%s yaw:%s", demo_tilt_sensor_simple.cnt, limit, pitch, roll, yaw)
 
-    movehub.tilt_sensor.subscribe(callback, mode=TiltSensor.MODE_3AXIS_FULL)
+    movehub.tilt_sensor.subscribe(callback, mode=TiltSensor.MODE_3AXIS_ACCEL)
     while demo_tilt_sensor_simple.cnt < limit:
         time.sleep(1)
 
@@ -111,16 +115,16 @@ def demo_color_sensor(movehub):
         demo_color_sensor.cnt += 1
         log.info("#%s/%s: Color %s, distance %s", demo_color_sensor.cnt, limit, COLORS[color], distance)
 
-    movehub.color_distance_sensor.subscribe(callback)
+    movehub.vision_sensor.subscribe(callback)
     while demo_color_sensor.cnt < limit:
         time.sleep(1)
 
-    movehub.color_distance_sensor.unsubscribe(callback)
+    movehub.vision_sensor.unsubscribe(callback)
 
 
 def demo_motor_sensors(movehub):
     log.info("Motor rotation sensors test. Rotate all available motors once")
-    demo_motor_sensors.states = {movehub.motor_A: None, movehub.motor_B: None}
+    demo_motor_sensors.states = {movehub.motor_A: 0, movehub.motor_B: 0, movehub.motor_external: 0}
 
     def callback_a(param1):
         demo_motor_sensors.states[movehub.motor_A] = param1
@@ -141,14 +145,13 @@ def demo_motor_sensors(movehub):
         demo_motor_sensors.states[movehub.motor_external] = None
         movehub.motor_external.subscribe(callback_e)
 
-    while None in demo_motor_sensors.states.values():
+    while not all([x is not None and abs(x) > 30 for x in demo_motor_sensors.states.values()]):
         time.sleep(1)
 
     movehub.motor_A.unsubscribe(callback_a)
     movehub.motor_B.unsubscribe(callback_b)
 
     if movehub.motor_external is not None:
-        demo_motor_sensors.states[movehub.motor_external] = None
         movehub.motor_external.unsubscribe(callback_e)
 
 
@@ -159,40 +162,103 @@ def demo_voltage(movehub):
     def callback2(value):
         log.info("Voltage: %s", value)
 
-    movehub.amperage.subscribe(callback1, mode=Amperage.MODE1, granularity=0)
-    movehub.amperage.subscribe(callback1, mode=Amperage.MODE1, granularity=1)
+    movehub.current.subscribe(callback1, mode=Current.CURRENT_L, granularity=0)
+    movehub.current.subscribe(callback1, mode=Current.CURRENT_L, granularity=1)
 
-    movehub.voltage.subscribe(callback2, mode=Voltage.MODE1, granularity=0)
-    movehub.voltage.subscribe(callback2, mode=Voltage.MODE1, granularity=1)
+    movehub.voltage.subscribe(callback2, mode=Voltage.VOLTAGE_L, granularity=0)
+    movehub.voltage.subscribe(callback2, mode=Voltage.VOLTAGE_L, granularity=1)
     time.sleep(5)
-    movehub.amperage.unsubscribe(callback1)
+    movehub.current.unsubscribe(callback1)
     movehub.voltage.unsubscribe(callback2)
 
 
 def demo_all(movehub):
-    demo_voltage(movehub)
-    demo_led_colors(movehub)
     demo_motors_timed(movehub)
     demo_motors_angled(movehub)
     demo_port_cd_motor(movehub)
+    demo_led_colors(movehub)
     demo_tilt_sensor_simple(movehub)
     demo_tilt_sensor_precise(movehub)
     demo_color_sensor(movehub)
     demo_motor_sensors(movehub)
+    demo_voltage(movehub)
+
+
+DEMO_CHOICES = {
+    'all': demo_all,
+    'voltage': demo_voltage,
+    'led_colors': demo_led_colors,
+    'motors_timed': demo_motors_timed,
+    'motors_angled': demo_motors_angled,
+    'port_cd_motor': demo_port_cd_motor,
+    'tilt_sensor': demo_tilt_sensor_simple,
+    'tilt_sensor_precise': demo_tilt_sensor_precise,
+    'color_sensor': demo_color_sensor,
+    'motor_sensors': demo_motor_sensors,
+}
+
+
+def get_options():
+    import argparse
+    arg_parser = argparse.ArgumentParser(
+        description='Demonstrate move-hub communications',
+    )
+    arg_parser.add_argument(
+        '-c', '--connection',
+        default='auto://',
+        help='''Specify connection URL to use, `protocol://mac?param=X` with protocol in:
+    "gatt","pygatt","gattlib","gattool", "bluepy","bluegiga"'''
+    )
+    arg_parser.add_argument(
+        '-d', '--demo',
+        default='all',
+        choices=sorted(DEMO_CHOICES.keys()),
+        help="Run a particular demo, default all"
+    )
+    return arg_parser
+
+
+def connection_from_url(url):
+    import pylgbst
+    if url == 'auto://':
+        return None
+    try:
+        from urllib.parse import urlparse, parse_qs
+    except ImportError:
+        from urlparse import urlparse, parse_qs
+    parsed = urlparse(url)
+    name = 'get_connection_%s' % parsed.scheme
+    factory = getattr(pylgbst, name, None)
+    if not factory:
+        msg = "Unrecognised URL scheme/protocol, expect a get_connection_<protocol> in pylgbst: %s"
+        raise ValueError(msg % parsed.protocol)
+    params = {}
+    if parsed.netloc.strip():
+        params['hub_mac'] = parsed.netloc
+    for key, value in parse_qs(parsed.query).items():
+        if len(value) == 1:
+            params[key] = value[0]
+        else:
+            params[key] = value
+    return factory(
+        **params
+    )
 
 
 if __name__ == '__main__':
-    logging.basicConfig(level=logging.INFO)
-
+    logging.basicConfig(level=logging.INFO, format='%(relativeCreated)d\t%(levelname)s\t%(name)s\t%(message)s')
+    parser = get_options()
+    options = parser.parse_args()
+    parameters = {}
     try:
-        connection = DebugServerConnection()
-    except BaseException:
-        logging.debug("Failed to use debug server: %s", traceback.format_exc())
-        connection = get_connection_auto()
+        connection = connection_from_url(options.connection)  # get_connection_bleak(hub_name=MoveHub.DEFAULT_NAME)
+        parameters['connection'] = connection
+    except ValueError as err:
+        parser.error(err.args[0])
 
+    hub = MoveHub(**parameters)
     try:
-        hub = MoveHub(connection)
-        sleep(1)
-        # demo_all(hub)
+        demo = DEMO_CHOICES[options.demo]
+        demo(hub)
     finally:
-        connection.disconnect()
+        hub.disconnect()

examples/harmonograph/__init__.py

@@ -3,20 +3,13 @@ import traceback
 
 from pylgbst import get_connection_auto
 from pylgbst.comms import DebugServerConnection
-from pylgbst.movehub import MoveHub
+from pylgbst.hub import MoveHub
 
 if __name__ == '__main__':
     logging.basicConfig(level=logging.INFO)
-
+    hub = MoveHub()
     try:
-        conn = DebugServerConnection()
-    except BaseException:
-        logging.warning("Failed to use debug server: %s", traceback.format_exc())
-        conn = get_connection_auto()
-
-    hub = MoveHub(conn)
-    try:
-        hub.motor_AB.constant(0.45, 0.45)
+        hub.motor_AB.start_power(0.45, 0.45)
         hub.motor_external.angled(12590, 0.1)
         # time.sleep(180)
     finally:

examples/plotter/__init__.py

@@ -2,8 +2,7 @@ import logging
 import math
 import time
 
-from pylgbst.constants import COLOR_RED, COLOR_CYAN, COLORS
-from pylgbst.peripherals import ColorDistanceSensor
+from pylgbst.peripherals import VisionSensor, COLOR_RED, COLOR_CYAN, COLORS
 
 
 class Plotter(object):
@@ -13,7 +12,7 @@ class Plotter(object):
     def __init__(self, hub, base_speed=1.0):
         """
 
-        :type hub: pylgbst.movehub.MoveHub
+        :type hub: pylgbst.hub.MoveHub
         """
         self._hub = hub
         self.caret = self._hub.motor_A
@@ -36,27 +35,27 @@ class Plotter(object):
         self.is_tool_down = False
 
     def _reset_caret(self):
-        if not self._hub.color_distance_sensor:
+        if not self._hub.vision_sensor:
             logging.warning("No color/distance sensor, cannot center caret")
             return
 
-        self._hub.color_distance_sensor.subscribe(self._on_distance, mode=ColorDistanceSensor.COLOR_DISTANCE_FLOAT)
+        self._hub.vision_sensor.subscribe(self._on_distance, mode=VisionSensor.COLOR_DISTANCE_FLOAT)
         self.caret.timed(0.5, 1)
         try:
-            self.caret.constant(-1)
+            self.caret.start_power(-1)
             count = 0
             max_tries = 50
             while self._marker_color not in (COLOR_RED, COLOR_CYAN) and count < max_tries:
                 time.sleep(30.0 / max_tries)
                 count += 1
-            self._hub.color_distance_sensor.unsubscribe(self._on_distance)
+            self._hub.vision_sensor.unsubscribe(self._on_distance)
             clr = COLORS[self._marker_color] if self._marker_color else None
             logging.info("Centering tries: %s, color #%s", count, clr)
             if count >= max_tries:
                 raise RuntimeError("Failed to center caret")
         finally:
             self.caret.stop()
-            self._hub.color_distance_sensor.unsubscribe(self._on_distance)
+            self._hub.vision_sensor.unsubscribe(self._on_distance)
 
         if self._marker_color == COLOR_CYAN:
             self.move(-0.1, 0)
@@ -84,7 +83,7 @@ class Plotter(object):
     def finalize(self):
         if self.is_tool_down:
             self._tool_up()
-        self.both.stop(async=True)
+        self.both.stop()
 
     def _tool_down(self):
         self.is_tool_down = True
@@ -192,7 +191,7 @@ class Plotter(object):
             spa = speed_a * self.base_speed
             spb = -speed_b * self.base_speed * self.MOTOR_RATIO
             logging.info("Motor speeds: %.3f / %.3f", spa, spb)
-            self.both.constant(spa, spb)
+            self.both.start_power(spa, spb)
             time.sleep(dur)
 
     def spiral(self, rounds, growth):
@@ -216,7 +215,7 @@ class Plotter(object):
         for speed_a, speed_b, dur in speeds:
             spa = speed_a * self.base_speed
             spb = -speed_b * self.base_speed * self.MOTOR_RATIO
-            self.both.constant(spa, spb)
+            self.both.start_power(spa, spb)
             logging.info("Motor speeds: %.3f / %.3f", spa, spb)
             time.sleep(dur)
 

examples/plotter/try.py

@@ -1,14 +1,9 @@
 # coding=utf-8
 import logging
 import time
-import traceback
-
-import six
 
 from examples.plotter import Plotter
-from pylgbst import get_connection_auto
-from pylgbst.comms import DebugServerConnection
-from pylgbst.movehub import EncodedMotor, PORT_AB, PORT_C, PORT_A, PORT_B, MoveHub
+from pylgbst.hub import EncodedMotor, MoveHub
 from tests import HubMock
 
 
@@ -91,11 +86,11 @@ def try_speeds():
     speeds = [x * 1.0 / 10.0 for x in range(1, 11)]
     for s in speeds:
         logging.info("%s", s)
-        plotter.both.constant(s, -s)
+        plotter.both.start_power(s, -s)
         time.sleep(1)
     for s in reversed(speeds):
         logging.info("%s", s)
-        plotter.both.constant(-s, s)
+        plotter.both.start_power(-s, s)
         time.sleep(1)
 
 
@@ -182,16 +177,15 @@ def angles_experiment():
 
 
 class MotorMock(EncodedMotor):
-    def _wait_sync(self, async):
-        super(MotorMock, self)._wait_sync(True)
+    pass
 
 
 def get_hub_mock():
     hub = HubMock()
-    hub.motor_A = MotorMock(hub, PORT_A)
-    hub.motor_B = MotorMock(hub, PORT_B)
-    hub.motor_AB = MotorMock(hub, PORT_AB)
-    hub.motor_external = MotorMock(hub, PORT_C)
+    hub.motor_A = MotorMock(hub, MoveHub.PORT_A)
+    hub.motor_B = MotorMock(hub, MoveHub.PORT_B)
+    hub.motor_AB = MotorMock(hub, MoveHub.PORT_AB)
+    hub.motor_external = MotorMock(hub, MoveHub.PORT_C)
     return hub
 
 
@@ -220,13 +214,7 @@ if __name__ == '__main__':
     logging.basicConfig(level=logging.DEBUG)
     logging.getLogger('').setLevel(logging.DEBUG)
 
-    try:
-        conn = DebugServerConnection()
-    except BaseException:
-        logging.warning("Failed to use debug server: %s", traceback.format_exc())
-        conn = get_connection_auto()
-
-    hub = MoveHub(conn) if 1 else get_hub_mock()
+    hub = MoveHub() if 1 else get_hub_mock()
 
     plotter = Plotter(hub, 0.75)
     FIELD_WIDTH = 0.9

examples/sorter/__init__.py

@@ -1,10 +1,7 @@
 import logging
-import traceback
 
-from pylgbst import get_connection_auto
-from pylgbst.comms import DebugServerConnection
-from pylgbst.constants import COLORS, COLOR_YELLOW, COLOR_BLUE, COLOR_CYAN, COLOR_RED, COLOR_BLACK
-from pylgbst.movehub import MoveHub
+from pylgbst.hub import MoveHub
+from pylgbst.peripherals import COLOR_YELLOW, COLOR_BLUE, COLOR_CYAN, COLOR_RED, COLOR_BLACK, COLORS
 
 
 class ColorSorter(MoveHub):
@@ -16,7 +13,7 @@ class ColorSorter(MoveHub):
         self.color = 0
         self.distance = 10
         self._last_wheel_dir = 1
-        self.color_distance_sensor.subscribe(self.on_color)
+        self.vision_sensor.subscribe(self.on_color)
         self.queue = [None for _ in range(0, 1)]
 
     def on_color(self, colr, dist):
@@ -50,10 +47,9 @@ class ColorSorter(MoveHub):
         self._last_wheel_dir = wheel_dir
 
     def clear(self):
-        self.color_distance_sensor.unsubscribe(self.on_color)
-        if not self.motor_B.in_progress():
-            self.move_to_bucket(COLOR_BLACK)
-        self.motor_AB.stop(async=True)
+        self.vision_sensor.unsubscribe(self.on_color)
+        self.move_to_bucket(COLOR_BLACK)
+        self.motor_AB.stop()
 
     def tick(self):
         res = False
@@ -80,13 +76,7 @@ class ColorSorter(MoveHub):
 if __name__ == '__main__':
     logging.basicConfig(level=logging.INFO)
 
-    try:
-        conn = DebugServerConnection()
-    except BaseException:
-        logging.warning("Failed to use debug server: %s", traceback.format_exc())
-        conn = get_connection_auto()
-
-    sorter = ColorSorter(conn)
+    sorter = ColorSorter()
     empty = 0
     try:
         while True:

examples/tracker/__init__.py

@@ -9,10 +9,8 @@ import cv2
 import imutils as imutils
 from matplotlib import pyplot
 
-from pylgbst import get_connection_auto
-from pylgbst.comms import DebugServerConnection
-from pylgbst.constants import COLOR_RED, COLOR_BLUE, COLOR_YELLOW
-from pylgbst.movehub import MoveHub
+from pylgbst.hub import MoveHub
+from pylgbst.peripherals import COLOR_RED, COLOR_BLUE, COLOR_YELLOW
 
 cascades_dir = '/usr/share/opencv/haarcascades'
 face_cascade = cv2.CascadeClassifier(cascades_dir + '/haarcascade_frontalface_default.xml')
@@ -85,6 +83,7 @@ class FaceTracker(MoveHub):
         return res
 
     def _find_smile(self, cur_face):
+        roi_color = None
         if cur_face is not None:
             (x, y, w, h) = cur_face
             roi_color = self.cur_img[y:y + h, x:x + w]
@@ -114,8 +113,7 @@ class FaceTracker(MoveHub):
         if on and not self._is_smile_on:
             self._is_smile_on = True
             self.motor_B.angled(-90, 0.5)
-            if self.led.last_color_set != COLOR_RED:
-                self.led.set_color(COLOR_RED)
+            self.led.set_color(COLOR_RED)
 
         if not on and self._is_smile_on:
             self._is_smile_on = False
@@ -142,7 +140,7 @@ class FaceTracker(MoveHub):
         mask = cv2.erode(mask, None, iterations=5)
         mask = cv2.dilate(mask, None, iterations=5)
 
-        #if not (int(time.time()) % 2):
+        # if not (int(time.time()) % 2):
         #    self.cur_img = mask
 
         ret, thresh = cv2.threshold(mask, 20, 255, 0)
@@ -167,8 +165,8 @@ class FaceTracker(MoveHub):
         if abs(vert) < 0.15:
             vert = 0
 
-        self.motor_external.constant(horz)
-        self.motor_A.constant(-vert)
+        self.motor_external.start_power(horz)
+        self.motor_A.start_power(-vert)
 
     def main(self):
         thr = Thread(target=self.capture)
@@ -191,15 +189,15 @@ class FaceTracker(MoveHub):
 
     def _process_picture(self, plt):
         self.cur_face = self._find_face()
-        #self.cur_face = self._find_color()
+        # self.cur_face = self._find_color()
 
         if self.cur_face is None:
             self.motor_external.stop()
             self.motor_AB.stop()
-            if not self._is_smile_on and self.led.last_color_set != COLOR_BLUE:
+            if not self._is_smile_on:
                 self.led.set_color(COLOR_BLUE)
         else:
-            if not self._is_smile_on and self.led.last_color_set != COLOR_YELLOW:
+            if not self._is_smile_on:
                 self.led.set_color(COLOR_YELLOW)
 
             self._auto_pan(self.cur_face)
@@ -213,13 +211,7 @@ if __name__ == '__main__':
     logging.basicConfig(level=logging.INFO)
 
     try:
-        conn = DebugServerConnection()
-    except BaseException:
-        logging.debug("Failed to use debug server: %s", traceback.format_exc())
-        conn = get_connection_auto()
-
-    try:
-        hub = FaceTracker(conn)
+        hub = FaceTracker()
         hub.main()
     finally:
         pass

examples/tracker/color.json

@@ -1,4 +1,12 @@
 [
-  [125, 64, 64],
-  [145, 255, 250]
+  [
+    125,
+    64,
+    64
+  ],
+  [
+    145,
+    255,
+    250
+  ]
 ]

examples/vernie/__init__.py

@@ -6,8 +6,7 @@ import subprocess
 import time
 
 from pylgbst import *
-from pylgbst.comms import DebugServerConnection
-from pylgbst.movehub import MoveHub
+from pylgbst.hub import MoveHub
 
 try:
     import gtts
@@ -65,17 +64,11 @@ VERNIE_SINGLE_MOVE = 430
 
 class Vernie(MoveHub):
     def __init__(self, language='en'):
-        try:
-            conn = DebugServerConnection()
-        except BaseException:
-            logging.warning("Failed to use debug server: %s", traceback.format_exc())
-            conn = get_connection_auto()
-
-        super(Vernie, self).__init__(conn)
+        super(Vernie, self).__init__()
         self.language = language
 
         while True:
-            required_devices = (self.color_distance_sensor, self.motor_external)
+            required_devices = (self.vision_sensor, self.motor_external)
             if None not in required_devices:
                 break
             log.debug("Waiting for required devices to appear: %s", required_devices)

examples/vernie/android_remote.py

@@ -2,15 +2,15 @@
 To use it, install this android app:
    https://play.google.com/store/apps/details?id=com.mscino.sensornode
 Then open app on phone and choose "Stream" => "Stream live data (XML)".
-Check the "Accelerometer" option and put your IP address into corresponding filed.
+Check the "Accelerometer" option and put your IP address into corresponding field.
 Specify port there as 8999, and enable streaming. Then run this script on computer.
 """
 import logging
 import socket
 import time
 
-from . import Vernie
-from pylgbst.peripherals import ColorDistanceSensor
+from examples.vernie import Vernie
+from pylgbst.peripherals import VisionSensor
 
 host = ''
 port = 8999
@@ -54,7 +54,7 @@ robot = Vernie()
 robot.button.subscribe(on_btn)
 robot.motor_AB.stop()
 
-robot.color_distance_sensor.subscribe(on_distance, ColorDistanceSensor.DISTANCE_INCHES)
+robot.vision_sensor.subscribe(on_distance, VisionSensor.DISTANCE_INCHES)
 try:
     udp_sock.bind((host, port))
     time.sleep(1)
@@ -85,7 +85,7 @@ try:
         sa = round(c + b / divider, 1)
         sb = round(c - b / divider, 1)
         logging.info("SpeedA=%s, SpeedB=%s", sa, sb)
-        robot.motor_AB.constant(sa, sb)
+        robot.motor_AB.start_speed(sa, sb)
         # time.sleep(0.5)
 finally:
     robot.motor_AB.stop()

examples/vernie/go_towards_light.py

@@ -1,5 +1,7 @@
-from pylgbst.peripherals import ColorDistanceSensor
-from . import *
+import logging
+
+from examples.vernie import Vernie
+from pylgbst.peripherals import VisionSensor
 
 logging.basicConfig(level=logging.INFO)
 
@@ -10,7 +12,8 @@ criterion = min
 cur_luminosity = 0
 
 
-def on_change_lum(lumn):
+def on_change_lum(lumn, unknown):
+    del unknown
     global cur_luminosity
     cur_luminosity = lumn
 
@@ -29,7 +32,7 @@ def on_turn(angl):
 
 
 robot.button.subscribe(on_btn)
-robot.color_distance_sensor.subscribe(on_change_lum, ColorDistanceSensor.LUMINOSITY, granularity=1)
+robot.vision_sensor.subscribe(on_change_lum, VisionSensor.DEBUG, granularity=1)
 robot.motor_A.subscribe(on_turn, granularity=30)
 
 # TODO: add bump detect to go back?
@@ -62,5 +65,5 @@ while running:
         logging.info("Luminosity is %.3f, moving towards it", cur_luminosity)
         robot.move(FORWARD, 1)
 
-robot.color_distance_sensor.unsubscribe(on_change_lum)
+robot.vision_sensor.unsubscribe(on_change_lum)
 robot.button.unsubscribe(on_btn)

examples/vernie/run_away_game.py

@@ -1,5 +1,5 @@
-from pylgbst.constants import COLOR_GREEN, COLOR_NONE
-from . import *
+from pylgbst.peripherals import COLOR_GREEN, COLOR_NONE
+from vernie import *
 
 robot = Vernie()
 running = True
@@ -11,7 +11,7 @@ def callback(color, distance):
     secs = (10 - distance + 1) / 10.0
     print("Distance is %.1f inches, I'm running back with %s%% speed!" % (distance, int(speed * 100)))
     if speed <= 1:
-        robot.motor_AB.timed(secs / 1, -speed, async=True)
+        robot.motor_AB.timed(secs / 1, -speed)
         robot.say("Ouch")
 
 
@@ -23,14 +23,11 @@ def on_btn(pressed):
 
 robot.led.set_color(COLOR_GREEN)
 robot.button.subscribe(on_btn)
-robot.color_distance_sensor.subscribe(callback)
+robot.vision_sensor.subscribe(callback)
 robot.say("Place your hand in front of sensor")
 
 while running:
     time.sleep(1)
 
-robot.color_distance_sensor.unsubscribe(callback)
+robot.vision_sensor.unsubscribe(callback)
 robot.button.unsubscribe(on_btn)
-robot.led.set_color(COLOR_NONE)
-while robot.led.in_progress():
-    time.sleep(1)

pylgbst/__init__.py

@@ -6,34 +6,57 @@ from pylgbst.comms import DebugServer
 log = logging.getLogger('pylgbst')
 
 
-def get_connection_bluegiga(controller=None, hub_mac=None):
+def get_connection_bluegiga(controller=None, hub_mac=None, hub_name=None):
+    del controller  # to prevent code analysis warning
     from pylgbst.comms.cpygatt import BlueGigaConnection
 
-    return BlueGigaConnection().connect(hub_mac)
+    return BlueGigaConnection().connect(hub_mac, hub_name)
 
 
-def get_connection_gattool(controller='hci0', hub_mac=None):
+def get_connection_gattool(controller='hci0', hub_mac=None, hub_name=None):
     from pylgbst.comms.cpygatt import GattoolConnection
 
-    return GattoolConnection(controller).connect(hub_mac)
+    return GattoolConnection(controller).connect(hub_mac, hub_name)
 
 
-def get_connection_gatt(controller='hci0', hub_mac=None):
+def get_connection_gatt(controller='hci0', hub_mac=None, hub_name=None):
     from pylgbst.comms.cgatt import GattConnection
 
-    return GattConnection(controller).connect(hub_mac)
+    return GattConnection(controller).connect(hub_mac, hub_name)
 
 
-def get_connection_gattlib(controller='hci0', hub_mac=None):
+def get_connection_gattlib(controller='hci0', hub_mac=None, hub_name=None):
     from pylgbst.comms.cgattlib import GattLibConnection
 
-    return GattLibConnection(controller).connect(hub_mac)
+    return GattLibConnection(controller).connect(hub_mac, hub_name)
 
 
-def get_connection_auto(controller='hci0', hub_mac=None):
+def get_connection_bluepy(controller='hci0', hub_mac=None, hub_name=None):
+    from pylgbst.comms.cbluepy import BluepyConnection
+
+    return BluepyConnection(controller).connect(hub_mac, hub_name)
+
+
+def get_connection_bleak(controller='hci0', hub_mac=None, hub_name=None):
+    """
+    Return connection based with Bleak API as an endpoint.
+
+    :param controller: Not used, kept for compatibility with others.
+    :param hub_mac: Optional Lego HUB MAC to connect to.
+    :return: Driver object.
+    """
+    del controller  # to prevent code analysis warning
+    from pylgbst.comms.cbleak import BleakDriver
+
+    return BleakDriver(hub_mac, hub_name)
+
+
+def get_connection_auto(controller='hci0', hub_mac=None, hub_name=None):
     fns = [
+        get_connection_bluepy,
         get_connection_bluegiga,
         get_connection_gatt,
+        get_connection_bleak,
         get_connection_gattool,
         get_connection_gattlib,
     ]
@@ -42,7 +65,7 @@ def get_connection_auto(controller='hci0', hub_mac=None):
     for fn in fns:
         try:
             logging.info("Trying %s", fn.__name__)
-            return fn(controller, hub_mac)
+            return fn(controller, hub_mac, hub_name)
         except KeyboardInterrupt:
             raise
         except BaseException:

pylgbst/comms/__init__.py

@@ -10,12 +10,17 @@ from abc import abstractmethod
 from binascii import unhexlify
 from threading import Thread
 
-from pylgbst.constants import MSG_DEVICE_SHUTDOWN, ENABLE_NOTIFICATIONS_HANDLE, ENABLE_NOTIFICATIONS_VALUE
+from pylgbst.messages import MsgHubAction
 from pylgbst.utilities import str2hex
 
 log = logging.getLogger('comms')
 
-LEGO_MOVE_HUB = "LEGO Move Hub"
+MOVE_HUB_HW_UUID_SERV = '00001623-1212-efde-1623-785feabcd123'
+MOVE_HUB_HW_UUID_CHAR = '00001624-1212-efde-1623-785feabcd123'
+ENABLE_NOTIFICATIONS_HANDLE = 0x000f
+ENABLE_NOTIFICATIONS_VALUE = b'\x01\x00'
+
+MOVE_HUB_HARDWARE_HANDLE = 0x0E
 
 
 class Connection(object):
@@ -40,6 +45,22 @@ class Connection(object):
     def enable_notifications(self):
         self.write(ENABLE_NOTIFICATIONS_HANDLE, ENABLE_NOTIFICATIONS_VALUE)
 
+    def _is_device_matched(self, address, dev_name, hub_mac, find_name):
+        assert hub_mac or find_name, 'You have to provide either hub_mac or hub_name in connection options'
+        log.debug("Checking device: %s, MAC: %s", dev_name, address)
+        matched = False
+        if address != "00:00:00:00:00:00":
+            if hub_mac:
+                if hub_mac.lower() == address.lower():
+                    matched = True
+            elif dev_name == find_name:
+                matched = True
+
+            if matched:
+                log.info("Found %s at %s", dev_name, address)
+
+        return matched
+
 
 class DebugServer(object):
     """
@@ -96,7 +117,7 @@ class DebugServer(object):
         self._check_shutdown(data)
 
     def _check_shutdown(self, data):
-        if data[5] == MSG_DEVICE_SHUTDOWN:
+        if data[5] == MsgHubAction.TYPE:
             log.warning("Device shutdown")
             self._running = False
 
@@ -118,7 +139,7 @@ class DebugServer(object):
                 buf = buf[buf.index("\n") + 1:]
 
                 if line:
-                    log.info("Cmd line: %s", line)
+                    log.debug("Cmd line: %s", line)
                     try:
                         self._handle_cmd(json.loads(line))
                     except KeyboardInterrupt:
@@ -192,3 +213,6 @@ class DebugServerConnection(Connection):
 
     def set_notify_handler(self, handler):
         self.notify_handler = handler
+
+    def is_alive(self):
+        return self.reader.isAlive()

pylgbst/comms/cbleak.py

@@ -0,0 +1,216 @@
+import asyncio
+import logging
+import queue
+import threading
+import time
+
+from bleak import BleakClient, discover
+
+from pylgbst.comms import Connection, MOVE_HUB_HW_UUID_CHAR
+
+log = logging.getLogger('comms-bleak')
+
+# Queues to handle request / responses. Acts as a buffer between API and async BLE driver
+resp_queue = queue.Queue()
+req_queue = queue.Queue()
+
+
+class BleakDriver(object):
+    """Driver that provides interface between API and Bleak."""
+
+    def __init__(self, hub_mac=None, hub_name=None):
+        """
+        Initialize new object of Bleak Driver class.
+
+        :param hub_mac: Optional Lego HUB MAC to connect to.
+        """
+        self.hub_mac = hub_mac
+        self.hub_name = hub_name
+        self._handler = None
+        self._abort = False
+        self._connection_thread = None
+        self._processing_thread = None
+
+    def set_notify_handler(self, handler):
+        """
+        Set handler function used to communicate with an API.
+
+        :param handler: Handler function called by driver when received data
+        :return: None
+        """
+        self._handler = handler
+
+    def enable_notifications(self):
+        """
+        Enable notifications, in our cases starts communication threads.
+
+        We cannot do this earlier, because API need to fist set notification handler.
+        :return: None
+        """
+        self._connection_thread = threading.Thread(target=lambda: asyncio.run(self._bleak_thread()))
+        self._connection_thread.daemon = True
+        self._connection_thread.start()
+
+        self._processing_thread = threading.Thread(target=self._processing)
+        self._processing_thread.daemon = True
+        self._processing_thread.start()
+
+    async def _bleak_thread(self):
+        bleak = BleakConnection()
+        await bleak.connect(self.hub_mac, self.hub_name)
+        await bleak.set_notify_handler(self._safe_handler)
+        # After connecting, need to send any data or hub will drop the connection,
+        # below command is Advertising name request update
+        await bleak.write_char(MOVE_HUB_HW_UUID_CHAR, bytearray([0x05, 0x00, 0x01, 0x01, 0x05]))
+        while not self._abort:
+            await asyncio.sleep(0.1)
+            if req_queue.qsize() != 0:
+                data = req_queue.get()
+                await bleak.write(data[0], data[1])
+
+        logging.info("Communications thread has exited")
+
+    @staticmethod
+    def _safe_handler(handler, data):
+        resp_queue.put((handler, data))
+
+    def _processing(self):
+        while not self._abort:
+            if resp_queue.qsize() != 0:
+                msg = resp_queue.get()
+                self._handler(msg[0], bytes(msg[1]))
+
+            time.sleep(0.01)
+        logging.info("Processing thread has exited")
+
+    def write(self, handle, data):
+        """
+        Send data to given handle number.
+
+        :param handle: Handle number that will be translated into characteristic uuid
+        :param data: data to send
+        :raises ConnectionError" When internal threads are not working
+        :return: None
+        """
+        if not self._connection_thread.is_alive() or not self._processing_thread.is_alive():
+            raise ConnectionError('Something went wrong, communication threads not functioning.')
+
+        req_queue.put((handle, data))
+
+    def disconnect(self):
+        """
+        Disconnect and stops communication threads.
+
+        :return: None
+        """
+        self._abort = True
+
+    def is_alive(self):
+        """
+        Indicate whether driver is functioning or not.
+
+        :return: True if driver is functioning; False otherwise.
+        """
+        if self._connection_thread is not None and self._processing_thread is not None:
+            return self._connection_thread.is_alive() and self._processing_thread.is_alive()
+        else:
+            return False
+
+
+class BleakConnection(Connection):
+    """Bleak driver for communicating with BLE device."""
+
+    def __init__(self):
+        """Initialize new instance of BleakConnection class."""
+        Connection.__init__(self)
+        self.loop = asyncio.get_event_loop()
+
+        self._device = None
+        self._client = None
+        logging.getLogger('bleak.backends.dotnet.client').setLevel(logging.WARNING)
+        logging.getLogger('bleak.backends.bluezdbus.client').setLevel(logging.WARNING)
+
+    async def connect(self, hub_mac=None, hub_name=None):
+        """
+        Connect to device.
+
+        :param hub_mac: Optional Lego HUB MAC to connect to.
+        :raises ConnectionError: When cannot connect to given MAC or name matching fails.
+        :return: None
+        """
+        log.info("Discovering devices... Press green button on Hub")
+        for i in range(0, 30):
+            devices = await discover(timeout=1)
+            log.debug("Devices: %s", devices)
+            for dev in devices:
+                log.debug(dev)
+                address = dev.address
+                name = dev.name
+                if self._is_device_matched(address, name, hub_mac, hub_name):
+                    log.info('Device matched: %r', dev)
+                    self._device = dev
+                    break
+            else:
+                continue
+
+            break
+        else:
+            raise ConnectionError('Device not found.')
+
+        self._client = BleakClient(self._device.address, self.loop)
+        status = await self._client.connect()
+        log.debug('Connection status: {status}'.format(status=status))
+
+    async def write(self, handle, data):
+        """
+        Send data to given handle number.
+
+        If handle cannot be found in service description, hardcoded LEGO uuid will be used.
+        :param handle: Handle number that will be translated into characteristic uuid
+        :param data: data to send
+        :return: None
+        """
+        log.debug('Request: {handle} {payload}'.format(handle=handle, payload=[hex(x) for x in data]))
+        desc = self._client.services.get_descriptor(handle)
+
+        if not isinstance(data, bytearray):
+            data = bytearray(data)
+
+        if desc is None:
+            # dedicated handle not found, try to send by using LEGO Move Hub default characteristic
+            await self._client.write_gatt_char(MOVE_HUB_HW_UUID_CHAR, data)
+        else:
+            await self._client.write_gatt_char(desc.characteristic_uuid, data)
+
+    async def write_char(self, characteristic_uuid, data):
+        """
+        Send data to given handle number.
+
+        :param characteristic_uuid: Characteristic uuid used to send data
+        :param data: data to send
+        :return: None
+        """
+        await self._client.write_gatt_char(characteristic_uuid, data)
+
+    async def set_notify_handler(self, handler):
+        """
+        Set notification handler.
+
+        :param handler: Handle function to be called when receive any data.
+        :return: None
+        """
+
+        def c(handle, data):
+            log.debug('Response: {handle} {payload}'.format(handle=handle, payload=[hex(x) for x in data]))
+            handler(handle, data)
+
+        await self._client.start_notify(MOVE_HUB_HW_UUID_CHAR, c)
+
+    def is_alive(self):
+        """
+        To keep compatibility with the driver interface.
+
+        This method does nothing.
+        :return: None.
+        """
+        pass

pylgbst/comms/cbluepy.py

@@ -0,0 +1,121 @@
+import logging
+import re
+from threading import Thread, Event
+
+from bluepy import btle
+
+from pylgbst.comms import Connection
+from pylgbst.utilities import str2hex, queue
+
+log = logging.getLogger('comms-bluepy')
+
+COMPLETE_LOCAL_NAME_ADTYPE = 9
+PROPAGATE_DISPATCHER_EXCEPTION = False
+
+
+def _get_iface_number(controller):
+    """bluepy uses iface numbers instead of full names."""
+    if not controller:
+        return None
+    m = re.search(r'hci(\d+)$', controller)
+    if not m:
+        raise ValueError('Cannot find iface number in {}.'.format(controller))
+    return int(m.group(1))
+
+
+class BluepyDelegate(btle.DefaultDelegate):
+    def __init__(self, handler):
+        btle.DefaultDelegate.__init__(self)
+
+        self._handler = handler
+
+    def handleNotification(self, cHandle, data):
+        log.debug('Incoming notification')
+        self._handler(cHandle, data)
+
+
+# We need a separate thread to wait for notifications, 
+# but calling peripheral's methods from different threads creates issues,
+# so we will wrap all the calls into a thread
+class BluepyThreadedPeripheral(object):
+    def __init__(self, addr, addrType, controller):
+        self._call_queue = queue.Queue()
+        self._addr = addr
+        self._addrType = addrType
+        self._iface_number = _get_iface_number(controller)
+
+        self._disconnect_event = Event()
+
+        self._dispatcher_thread = Thread(target=self._dispatch_calls)
+        self._dispatcher_thread.setDaemon(True)
+        self._dispatcher_thread.setName("Bluepy call dispatcher")
+        self._dispatcher_thread.start()
+
+    def _dispatch_calls(self):
+        self._peripheral = btle.Peripheral(self._addr, self._addrType, self._iface_number)
+        try:
+            while not self._disconnect_event.is_set():
+                try:
+                    try:
+                        method = self._call_queue.get(False)
+                        method()
+                    except queue.Empty:
+                        pass
+                    self._peripheral.waitForNotifications(1.)
+                except Exception as ex:
+                    log.exception('Exception in call dispatcher thread', exc_info=ex)
+                    if PROPAGATE_DISPATCHER_EXCEPTION:
+                        log.error("Terminating dispatcher thread.")
+                        raise
+        finally:
+            self._peripheral.disconnect()
+
+    def write(self, handle, data):
+        self._call_queue.put(lambda: self._peripheral.writeCharacteristic(handle, data))
+
+    def set_notify_handler(self, handler):
+        delegate = BluepyDelegate(handler)
+        self._call_queue.put(lambda: self._peripheral.withDelegate(delegate))
+
+    def disconnect(self):
+        self._disconnect_event.set()
+
+
+class BluepyConnection(Connection):
+    def __init__(self, controller='hci0'):
+        Connection.__init__(self)
+        self._peripheral = None  # :type BluepyThreadedPeripheral
+        self._controller = controller
+
+    def connect(self, hub_mac=None, hub_name=None):
+        log.debug("Trying to connect client to MoveHub with MAC: %s", hub_mac)
+        scanner = btle.Scanner()
+
+        while not self._peripheral:
+            log.info("Discovering devices...")
+            scanner.scan(1)
+            devices = scanner.getDevices()
+
+            for dev in devices:
+                address = dev.addr
+                address_type = dev.addrType
+                name = dev.getValueText(COMPLETE_LOCAL_NAME_ADTYPE)
+
+                if self._is_device_matched(address, name, hub_mac, hub_name):
+                    self._peripheral = BluepyThreadedPeripheral(address, address_type, self._controller)
+                    break
+
+        return self
+
+    def disconnect(self):
+        self._peripheral.disconnect()
+
+    def write(self, handle, data):
+        log.debug("Writing to handle %s: %s", handle, str2hex(data))
+        self._peripheral.write(handle, data)
+
+    def set_notify_handler(self, handler):
+        self._peripheral.set_notify_handler(handler)
+
+    def is_alive(self):
+        return True

pylgbst/comms/cgatt.py

@@ -5,8 +5,8 @@ from time import sleep
 
 import gatt
 
-from pylgbst.comms import Connection, LEGO_MOVE_HUB
-from pylgbst.constants import MOVE_HUB_HW_UUID_SERV, MOVE_HUB_HW_UUID_CHAR, MOVE_HUB_HARDWARE_HANDLE
+from pylgbst.comms import Connection, MOVE_HUB_HW_UUID_SERV, MOVE_HUB_HW_UUID_CHAR, \
+    MOVE_HUB_HARDWARE_HANDLE
 from pylgbst.utilities import str2hex
 
 log = logging.getLogger('comms-gatt')
@@ -85,10 +85,10 @@ class GattConnection(Connection):
             raise NotImplementedError("Gatt is not implemented for this platform")
 
         self._manager_thread = threading.Thread(target=self._manager.run)
-        self._manager_thread.setDaemon(False)
+        self._manager_thread.setDaemon(True)
         log.debug('Starting DeviceManager...')
 
-    def connect(self, hub_mac=None):
+    def connect(self, hub_mac=None, hub_name=None):
         self._manager_thread.start()
         self._manager.start_discovery()
 
@@ -100,8 +100,7 @@ class GattConnection(Connection):
             for dev in devices:
                 address = dev.mac_address
                 name = dev.alias()
-                if name == LEGO_MOVE_HUB or hub_mac == address:
-                    logging.info("Found %s at %s", name, address)
+                if self._is_device_matched(address, name, hub_mac, hub_name):
                     self._device = CustomDevice(address, self._manager)
                     break
 
@@ -112,6 +111,7 @@ class GattConnection(Connection):
         return self
 
     def disconnect(self):
+        self._manager.stop()
         self._device.disconnect()
 
     def write(self, handle, data):

pylgbst/comms/cgattlib.py

@@ -5,7 +5,7 @@ from threading import Thread
 
 from gattlib import DiscoveryService, GATTRequester
 
-from pylgbst.comms import Connection, LEGO_MOVE_HUB
+from pylgbst.comms import Connection
 from pylgbst.utilities import queue, str2hex
 
 log = logging.getLogger('comms-gattlib')
@@ -61,7 +61,7 @@ class GattLibConnection(Connection):
         self.requester = None
         self._iface = bt_iface_name
 
-    def connect(self, hub_mac=None):
+    def connect(self, hub_mac=None, hub_name=None):
         service = DiscoveryService(self._iface)
 
         while not self.requester:
@@ -70,8 +70,7 @@ class GattLibConnection(Connection):
             log.debug("Devices: %s", devices)
 
             for address, name in devices.items():
-                if name == LEGO_MOVE_HUB or hub_mac == address:
-                    logging.info("Found %s at %s", name, address)
+                if self._is_device_matched(address, name, hub_mac, hub_name):
                     self.requester = Requester(address, True, self._iface)
                     break
 

pylgbst/comms/cpygatt.py

@@ -2,8 +2,7 @@ import logging
 
 import pygatt
 
-from pylgbst.comms import Connection, LEGO_MOVE_HUB
-from pylgbst.constants import MOVE_HUB_HW_UUID_CHAR
+from pylgbst.comms import Connection, MOVE_HUB_HW_UUID_CHAR
 from pylgbst.utilities import str2hex
 
 log = logging.getLogger('comms-pygatt')
@@ -21,21 +20,21 @@ class GattoolConnection(Connection):
         self.backend = lambda: pygatt.GATTToolBackend(hci_device=controller)
         self._conn_hnd = None
 
-    def connect(self, hub_mac=None):
+    def connect(self, hub_mac=None, hub_name=None):
         log.debug("Trying to connect client to MoveHub with MAC: %s", hub_mac)
         adapter = self.backend()
-        adapter.start()
+        adapter.start()  # enable or disable restart? What's the best?
 
         while not self._conn_hnd:
             log.info("Discovering devices...")
             devices = adapter.scan(1)
             log.debug("Devices: %s", devices)
 
+            # Pass each device found to _is_device_matched( ) to see if it the device we want
             for dev in devices:
                 address = dev['address']
                 name = dev['name']
-                if name == LEGO_MOVE_HUB or hub_mac == address:
-                    logging.info("Found %s at %s", name, address)
+                if self._is_device_matched(address, name, hub_mac, hub_name):
                     self._conn_hnd = adapter.connect(address)
                     break
 

pylgbst/constants.py

@@ -1,118 +0,0 @@
-# GENERAL
-ENABLE_NOTIFICATIONS_HANDLE = 0x000f
-ENABLE_NOTIFICATIONS_VALUE = b'\x01\x00'
-MOVE_HUB_HARDWARE_HANDLE = 0x0E
-MOVE_HUB_HW_UUID_SERV = '00001623-1212-efde-1623-785feabcd123'
-MOVE_HUB_HW_UUID_CHAR = '00001624-1212-efde-1623-785feabcd123'
-
-PACKET_VER = 0x01
-LEGO_MOVE_HUB = "LEGO Move Hub"
-
-# PORTS
-PORT_C = 0x01
-PORT_D = 0x02
-PORT_LED = 0x32
-PORT_A = 0x37
-PORT_B = 0x38
-PORT_AB = 0x39
-PORT_TILT_SENSOR = 0x3A
-PORT_AMPERAGE = 0x3B
-PORT_VOLTAGE = 0x3C
-
-PORTS = {
-    PORT_A: "A",
-    PORT_B: "B",
-    PORT_AB: "AB",
-    PORT_C: "C",
-    PORT_D: "D",
-    PORT_LED: "LED",
-    PORT_TILT_SENSOR: "TILT_SENSOR",
-    PORT_AMPERAGE: "AMPERAGE",
-    PORT_VOLTAGE: "VOLTAGE",
-}
-
-# PACKET TYPES
-MSG_DEVICE_INFO = 0x01
-# 0501010305 gives 090001030600000010
-MSG_DEVICE_SHUTDOWN = 0x02  # sent when hub shuts down by button hold
-MSG_PING_RESPONSE = 0x03
-MSG_PORT_INFO = 0x04
-MSG_PORT_CMD_ERROR = 0x05
-MSG_SET_PORT_VAL = 0x81
-MSG_PORT_STATUS = 0x82
-MSG_SENSOR_SUBSCRIBE = 0x41
-MSG_SENSOR_SOMETHING1 = 0x42  # it is seen close to sensor subscribe commands. Subscription options? Initial value?
-MSG_SENSOR_DATA = 0x45
-MSG_SENSOR_SUBSCRIBE_ACK = 0x47
-
-# DEVICE TYPES
-DEV_VOLTAGE = 0x14
-DEV_AMPERAGE = 0x15
-DEV_LED = 0x17
-DEV_DCS = 0x25
-DEV_IMOTOR = 0x26
-DEV_MOTOR = 0x27
-DEV_TILT_SENSOR = 0x28
-
-DEVICE_TYPES = {
-    DEV_DCS: "DISTANCE_COLOR_SENSOR",
-    DEV_IMOTOR: "IMOTOR",
-    DEV_MOTOR: "MOTOR",
-    DEV_TILT_SENSOR: "TILT_SENSOR",
-    DEV_LED: "LED",
-    DEV_AMPERAGE: "AMPERAGE",
-    DEV_VOLTAGE: "VOLTAGE",
-}
-
-# NOTIFICATIONS
-STATUS_STARTED = 0x01
-STATUS_CONFLICT = 0x05
-STATUS_FINISHED = 0x0a
-STATUS_INPROGRESS = 0x0c  # FIXME: not sure about description
-STATUS_INTERRUPTED = 0x0e  # FIXME:  not sure about description
-
-# COLORS
-COLOR_BLACK = 0x00
-COLOR_PINK = 0x01
-COLOR_PURPLE = 0x02
-COLOR_BLUE = 0x03
-COLOR_LIGHTBLUE = 0x04
-COLOR_CYAN = 0x05
-COLOR_GREEN = 0x06
-COLOR_YELLOW = 0x07
-COLOR_ORANGE = 0x09
-COLOR_RED = 0x09
-COLOR_WHITE = 0x0a
-COLOR_NONE = 0xFF
-COLORS = {
-    COLOR_BLACK: "BLACK",
-    COLOR_PINK: "PINK",
-    COLOR_PURPLE: "PURPLE",
-    COLOR_BLUE: "BLUE",
-    COLOR_LIGHTBLUE: "LIGHTBLUE",
-    COLOR_CYAN: "CYAN",
-    COLOR_GREEN: "GREEN",
-    COLOR_YELLOW: "YELLOW",
-    COLOR_ORANGE: "ORANGE",
-    COLOR_RED: "RED",
-    COLOR_WHITE: "WHITE",
-    COLOR_NONE: "NONE"
-}
-
-# DEVICE INFO
-INFO_DEVICE_NAME = 0x01
-INFO_BUTTON_STATE = 0x02
-INFO_FIRMWARE_VERSION = 0x03
-INFO_SOME4 = 0x04
-INFO_SOME5_JITTERING = 0x05
-INFO_SOME6 = 0x06
-INFO_SOME7 = 0x07
-INFO_MANUFACTURER = 0x08
-INFO_HW_VERSION = 0x09
-INFO_SOME10 = 0x0a
-INFO_SOME11 = 0x0b
-INFO_SOME12 = 0x0c
-
-INFO_ACTION_SUBSCRIBE = 0x02
-INFO_ACTION_UNSUBSCRIBE = 0x03
-INFO_ACTION_GET = 0x05

pylgbst/hub.py

@@ -0,0 +1,292 @@
+import threading
+import time
+
+from pylgbst import get_connection_auto
+from pylgbst.messages import *
+from pylgbst.peripherals import *
+from pylgbst.utilities import queue
+from pylgbst.utilities import str2hex, usbyte, ushort
+
+log = logging.getLogger('hub')
+
+PERIPHERAL_TYPES = {
+    MsgHubAttachedIO.DEV_MOTOR: Motor,
+    MsgHubAttachedIO.DEV_MOTOR_EXTERNAL_TACHO: EncodedMotor,
+    MsgHubAttachedIO.DEV_MOTOR_INTERNAL_TACHO: EncodedMotor,
+    MsgHubAttachedIO.DEV_VISION_SENSOR: VisionSensor,
+    MsgHubAttachedIO.DEV_RGB_LIGHT: LEDRGB,
+    MsgHubAttachedIO.DEV_TILT_EXTERNAL: TiltSensor,
+    MsgHubAttachedIO.DEV_TILT_INTERNAL: TiltSensor,
+    MsgHubAttachedIO.DEV_CURRENT: Current,
+    MsgHubAttachedIO.DEV_VOLTAGE: Voltage,
+}
+
+
+class Hub(object):
+    """
+    :type connection: pylgbst.comms.Connection
+    :type peripherals: dict[int,Peripheral]
+    """
+    HUB_HARDWARE_HANDLE = 0x0E
+
+    def __init__(self, connection=None):
+        self._msg_handlers = []
+        self.peripherals = {}
+        self._sync_request = None
+        self._sync_replies = queue.Queue(1)
+        self._sync_lock = threading.Lock()
+
+        self.add_message_handler(MsgHubAttachedIO, self._handle_device_change)
+        self.add_message_handler(MsgPortValueSingle, self._handle_sensor_data)
+        self.add_message_handler(MsgPortValueCombined, self._handle_sensor_data)
+        self.add_message_handler(MsgGenericError, self._handle_error)
+        self.add_message_handler(MsgHubAction, self._handle_action)
+
+        if not connection:
+            connection = get_connection_auto()  # TODO: how to identify the hub?
+        self.connection = connection
+        self.connection.set_notify_handler(self._notify)
+        self.connection.enable_notifications()
+
+    def __del__(self):
+        if self.connection and self.connection.is_alive():
+            self.connection.disconnect()
+
+    def add_message_handler(self, classname, callback):
+        self._msg_handlers.append((classname, callback))
+
+    def send(self, msg):
+        """
+        :type msg: pylgbst.messages.DownstreamMsg
+        :rtype: pylgbst.messages.UpstreamMsg
+        """
+        log.debug("Send message: %r", msg)
+        msgbytes = msg.bytes()
+        if msg.needs_reply:
+            with self._sync_lock:
+                assert not self._sync_request, "Pending request %r while trying to put %r" % (self._sync_request, msg)
+                self._sync_request = msg
+                log.debug("Waiting for sync reply to %r...", msg)
+
+            self.connection.write(self.HUB_HARDWARE_HANDLE, msgbytes)
+            resp = self._sync_replies.get()
+            log.debug("Fetched sync reply: %r", resp)
+            if isinstance(resp, MsgGenericError):
+                raise RuntimeError(resp.message())
+            return resp
+        else:
+            self.connection.write(self.HUB_HARDWARE_HANDLE, msgbytes)
+            return None
+
+    def _notify(self, handle, data):
+        log.debug("Notification on %s: %s", handle, str2hex(data))
+
+        msg = self._get_upstream_msg(data)
+
+        with self._sync_lock:
+            if self._sync_request:
+                if self._sync_request.is_reply(msg):
+                    log.debug("Found matching upstream msg: %r", msg)
+                    self._sync_replies.put(msg)
+                    self._sync_request = None
+
+        for msg_class, handler in self._msg_handlers:
+            if isinstance(msg, msg_class):
+                log.debug("Handling msg with %s: %r", handler, msg)
+                handler(msg)
+
+    def _get_upstream_msg(self, data):
+        msg_type = usbyte(data, 2)
+        msg = None
+        for msg_kind in UPSTREAM_MSGS:
+            if msg_type == msg_kind.TYPE:
+                msg = msg_kind.decode(data)
+                log.debug("Decoded message: %r", msg)
+                break
+        assert msg
+        return msg
+
+    def _handle_error(self, msg):
+        log.warning("Command error: %s", msg.message())
+        with self._sync_lock:
+            if self._sync_request:
+                self._sync_request = None
+                self._sync_replies.put(msg)
+
+    def _handle_action(self, msg):
+        """
+        :type msg: MsgHubAction
+        """
+        if msg.action == MsgHubAction.UPSTREAM_DISCONNECT:
+            log.warning("Hub disconnects")
+            self.connection.disconnect()
+        elif msg.action == MsgHubAction.UPSTREAM_SHUTDOWN:
+            log.warning("Hub switches off")
+            self.connection.disconnect()
+
+    def _handle_device_change(self, msg):
+        if msg.event == MsgHubAttachedIO.EVENT_DETACHED:
+            log.debug("Detaching peripheral: %s", self.peripherals[msg.port])
+            self.peripherals.pop(msg.port)
+            return
+
+        assert msg.event in (msg.EVENT_ATTACHED, msg.EVENT_ATTACHED_VIRTUAL)
+        port = msg.port
+        dev_type = ushort(msg.payload, 0)
+
+        if dev_type in PERIPHERAL_TYPES:
+            self.peripherals[port] = PERIPHERAL_TYPES[dev_type](self, port)
+        else:
+            log.warning("Have not dedicated class for peripheral type %x on port %x", dev_type, port)
+            self.peripherals[port] = Peripheral(self, port)
+
+        log.info("Attached peripheral: %s", self.peripherals[msg.port])
+
+        if msg.event == msg.EVENT_ATTACHED:
+            hw_revision = reversed([usbyte(msg.payload, x) for x in range(2, 6)])
+            sw_revision = reversed([usbyte(msg.payload, x) for x in range(6, 10)])
+            # what to do with this info? it's useless, I guess
+            del hw_revision, sw_revision
+        elif msg.event == msg.EVENT_ATTACHED_VIRTUAL:
+            self.peripherals[port].virtual_ports = (usbyte(msg.payload, 2), usbyte(msg.payload, 3))
+
+    def _handle_sensor_data(self, msg):
+        assert isinstance(msg, (MsgPortValueSingle, MsgPortValueCombined))
+        if msg.port not in self.peripherals:
+            log.warning("Notification on port with no device: %s", msg.port)
+            return
+
+        device = self.peripherals[msg.port]
+        device.queue_port_data(msg)
+
+    def disconnect(self):
+        self.send(MsgHubAction(MsgHubAction.DISCONNECT))
+
+    def switch_off(self):
+        self.send(MsgHubAction(MsgHubAction.SWITCH_OFF))
+
+
+class MoveHub(Hub):
+    """
+    Class implementing Lego Boost's MoveHub specifics
+
+    :type led: LEDRGB
+    :type tilt_sensor: TiltSensor
+    :type button: Button
+    :type current: Current
+    :type voltage: Voltage
+    :type vision_sensor: pylgbst.peripherals.VisionSensor
+    :type port_C: Peripheral
+    :type port_D: Peripheral
+    :type motor_A: EncodedMotor
+    :type motor_B: EncodedMotor
+    :type motor_AB: EncodedMotor
+    :type motor_external: EncodedMotor
+    """
+
+    DEFAULT_NAME = "LEGO Move Hub"
+
+    # PORTS
+    PORT_A = 0x00
+    PORT_B = 0x01
+    PORT_C = 0x02
+    PORT_D = 0x03
+    PORT_AB = 0x10
+    PORT_LED = 0x32
+    PORT_TILT_SENSOR = 0x3A
+    PORT_CURRENT = 0x3B
+    PORT_VOLTAGE = 0x3C
+
+    # noinspection PyTypeChecker
+    def __init__(self, connection=None):
+        self._comm_lock = threading.RLock()
+        if connection is None:
+            connection = get_connection_auto(hub_name=self.DEFAULT_NAME)
+
+        super(MoveHub, self).__init__(connection)
+        self.info = {}
+
+        # shorthand fields
+        self.button = Button(self)
+        self.led = None
+        self.current = None
+        self.voltage = None
+        self.motor_A = None
+        self.motor_B = None
+        self.motor_AB = None
+        self.vision_sensor = None
+        self.tilt_sensor = None
+        self.motor_external = None
+        self.port_C = None
+        self.port_D = None
+
+        self._wait_for_devices()
+        self._report_status()
+
+    def _wait_for_devices(self, get_dev_set=None):
+        if not get_dev_set:
+            get_dev_set = lambda: (self.motor_A, self.motor_B, self.motor_AB, self.led, self.tilt_sensor,
+                                   self.current, self.voltage)
+        for num in range(0, 100):
+            devices = get_dev_set()
+            if all(devices):
+                log.debug("All devices are present: %s", devices)
+                return
+            log.debug("Waiting for builtin devices to appear: %s", devices)
+            time.sleep(0.1)
+        log.warning("Got only these devices: %s", get_dev_set())
+
+    def _report_status(self):
+        # maybe add firmware version
+        name = self.send(MsgHubProperties(MsgHubProperties.ADVERTISE_NAME, MsgHubProperties.UPD_REQUEST))
+        mac = self.send(MsgHubProperties(MsgHubProperties.PRIMARY_MAC, MsgHubProperties.UPD_REQUEST))
+        log.info("%s on %s", name.payload, str2hex(mac.payload))
+
+        voltage = self.send(MsgHubProperties(MsgHubProperties.VOLTAGE_PERC, MsgHubProperties.UPD_REQUEST))
+        assert isinstance(voltage, MsgHubProperties)
+        log.info("Voltage: %s%%", usbyte(voltage.parameters, 0))
+
+        voltage = self.send(MsgHubAlert(MsgHubAlert.LOW_VOLTAGE, MsgHubAlert.UPD_REQUEST))
+        assert isinstance(voltage, MsgHubAlert)
+        if not voltage.is_ok():
+            log.warning("Low voltage, check power source (maybe replace battery)")
+
+    # noinspection PyTypeChecker
+    def _handle_device_change(self, msg):
+        with self._comm_lock:
+            super(MoveHub, self)._handle_device_change(msg)
+            if isinstance(msg, MsgHubAttachedIO) and msg.event != MsgHubAttachedIO.EVENT_DETACHED:
+                port = msg.port
+                if port == self.PORT_A:
+                    self.motor_A = self.peripherals[port]
+                elif port == self.PORT_B:
+                    self.motor_B = self.peripherals[port]
+                elif port == self.PORT_AB:
+                    self.motor_AB = self.peripherals[port]
+                elif port == self.PORT_C:
+                    self.port_C = self.peripherals[port]
+                elif port == self.PORT_D:
+                    self.port_D = self.peripherals[port]
+                elif port == self.PORT_LED:
+                    self.led = self.peripherals[port]
+                elif port == self.PORT_TILT_SENSOR:
+                    self.tilt_sensor = self.peripherals[port]
+                elif port == self.PORT_CURRENT:
+                    self.current = self.peripherals[port]
+                elif port == self.PORT_VOLTAGE:
+                    self.voltage = self.peripherals[port]
+
+                if type(self.peripherals[port]) == VisionSensor:
+                    self.vision_sensor = self.peripherals[port]
+                elif type(self.peripherals[port]) == EncodedMotor \
+                        and port not in (self.PORT_A, self.PORT_B, self.PORT_AB):
+                    self.motor_external = self.peripherals[port]
+
+
+class TrainHub(Hub):
+    DEFAULT_NAME = 'TrainHub'
+
+    def __init__(self, connection=None):
+        if connection is None:
+            connection = get_connection_auto(hub_name=self.DEFAULT_NAME)
+        super(TrainHub, self).__init__(connection)

pylgbst/messages.py

@@ -0,0 +1,745 @@
+import logging
+from struct import pack, unpack
+
+from pylgbst.utilities import str2hex
+
+log = logging.getLogger('hub')
+
+
+class Message(object):
+    TYPE = None
+
+    def __init__(self):
+        self.hub_id = 0x00  # not used according to official doc
+        self.payload = b""
+
+    def bytes(self):
+        """
+        see https://lego.github.io/lego-ble-wireless-protocol-docs/#common-message-header
+        """
+        msglen = len(self.payload) + 3
+        assert msglen < 127, "TODO: handle longer messages with 2-byte len"
+        return pack("<B", msglen) + pack("<B", self.hub_id) + pack("<B", self.TYPE) + self.payload
+
+    def __repr__(self):
+        # assert self.bytes()  # to trigger any field changes
+        data = self.__dict__
+        data = {x: (str2hex(y) if isinstance(y, bytes) else y)
+                for x, y in data.items()
+                if x not in ('hub_id',)}
+        return self.__class__.__name__ + "(%s)" % data
+
+
+class DownstreamMsg(Message):
+
+    def __init__(self):
+        super(DownstreamMsg, self).__init__()
+        self.needs_reply = False
+
+    def is_reply(self, msg):
+        del msg
+        return False
+
+
+class UpstreamMsg(Message):
+
+    def __init__(self):
+        super(UpstreamMsg, self).__init__()
+
+    @classmethod
+    def decode(cls, data):
+        """
+        see https://lego.github.io/lego-ble-wireless-protocol-docs/#common-message-header
+        """
+        msg = cls()
+        msg.payload = data
+        msglen = msg._byte()
+        assert msglen < 127, "TODO: handle longer messages with 2-byte len"
+        hub_id = msg._byte()
+        assert hub_id == 0
+        msg_type = msg._byte()
+        assert cls.TYPE == msg_type, "Message type does not match: %x!=%x" % (cls.TYPE, msg_type)
+        assert isinstance(msg.payload, (bytes, bytearray))
+        return msg
+
+    def __shift(self, vtype, vlen):
+        val = self.payload[0:vlen]
+        self.payload = self.payload[vlen:]
+        return unpack("<" + vtype, val)[0]
+
+    def _byte(self):
+        return self.__shift("B", 1)
+
+    def _short(self):
+        return self.__shift("H", 2)
+
+    def _long(self):
+        return self.__shift("I", 4)
+
+    def _float(self):
+        return self.__shift("f", 4)
+
+    def _bits_list(self, val):
+        res = []
+        x = 1
+        for i in range(16 + 1):
+            if val & x:
+                res.append(i)
+            x <<= 1
+        return res
+
+
+class MsgHubProperties(DownstreamMsg, UpstreamMsg):
+    """
+    https://lego.github.io/lego-ble-wireless-protocol-docs/index.html#hub-properties
+    """
+    TYPE = 0x01
+
+    ADVERTISE_NAME = 0x01
+    BUTTON = 0x02
+    FW_VERSION = 0x03
+    HW_VERSION = 0x04
+    RSSI = 0x05
+    VOLTAGE_PERC = 0x06
+    BATTERY_TYPE = 0x07
+    MANUFACTURER = 0x08
+    RADIO_FW_VERSION = 0x09
+    WIRELESS_PROTO_VERSION = 0x0A
+    SYSTEM_TYPE_ID = 0x0B
+    HW_NETW_ID = 0x0C
+    PRIMARY_MAC = 0x0D
+    SECONDARY_MAC = 0x0E
+    HARDWARE_NETWORK_FAMILY = 0x0F
+
+    SET = 0x01
+    UPD_ENABLE = 0x02
+    UPD_DISABLE = 0x03
+    RESET = 0x04
+    UPD_REQUEST = 0x05
+    UPSTREAM_UPDATE = 0x06
+
+    def __init__(self, prop=None, operation=None, parameters=b""):
+        super(MsgHubProperties, self).__init__()
+
+        self.property = prop
+        self.operation = operation
+        self.parameters = parameters
+
+    def bytes(self):
+        if self.operation in (self.UPD_REQUEST, self.UPD_ENABLE):
+            self.needs_reply = True
+        self.payload = pack("<B", self.property) + pack("<B", self.operation) + self.parameters
+        return super(MsgHubProperties, self).bytes()
+
+    @classmethod
+    def decode(cls, data):
+        msg = super(MsgHubProperties, cls).decode(data)
+        assert isinstance(msg, MsgHubProperties)
+        msg.property = msg._byte()
+        msg.operation = msg._byte()
+        msg.parameters = msg.payload
+        return msg
+
+    def is_reply(self, msg):
+        return isinstance(msg, MsgHubProperties) \
+               and msg.operation == self.UPSTREAM_UPDATE and msg.property == self.property
+
+
+class MsgHubAction(DownstreamMsg, UpstreamMsg):
+    """
+    https://lego.github.io/lego-ble-wireless-protocol-docs/index.html#hub-actions
+    """
+    TYPE = 0x02
+
+    SWITCH_OFF = 0x01
+    DISCONNECT = 0x02
+    VCC_PORT_CONTROL_ON = 0x03
+    VCC_PORT_CONTROL_OFF = 0x04
+    BUSY_INDICATION_ON = 0x05
+    BUSY_INDICATION_OFF = 0x06
+    SWITCH_OFF_IMMEDIATELY = 0x2F
+
+    UPSTREAM_SHUTDOWN = 0x30
+    UPSTREAM_DISCONNECT = 0x31
+    UPSTREAM_BOOT_MODE = 0x32
+
+    def __init__(self, action=None):
+        super(MsgHubAction, self).__init__()
+        self.action = action
+
+    def bytes(self):
+        self.payload = pack("<B", self.action)
+        self.needs_reply = self.action in (self.DISCONNECT, self.SWITCH_OFF)
+        return super(MsgHubAction, self).bytes()
+
+    def is_reply(self, msg):
+        if not isinstance(msg, MsgHubAction):
+            raise TypeError("Unexpected message type: %s" % (msg.__class__,))
+        if self.action == self.DISCONNECT and msg.action == self.UPSTREAM_DISCONNECT:
+            return True
+
+        if self.action == self.SWITCH_OFF and msg.action == self.UPSTREAM_SHUTDOWN:
+            return True
+
+    @classmethod
+    def decode(cls, data):
+        msg = super(MsgHubAction, cls).decode(data)
+        assert isinstance(msg, MsgHubAction)
+        msg.action = msg._byte()
+        return msg
+
+
+class MsgHubAlert(DownstreamMsg, UpstreamMsg):
+    """
+    https://lego.github.io/lego-ble-wireless-protocol-docs/index.html#hub-alerts
+    """
+    TYPE = 0x03
+
+    LOW_VOLTAGE = 0x01
+    HIGH_CURRENT = 0x02
+    LOW_SIGNAL = 0x03
+    OVER_POWER = 0x04
+
+    DESCR = {
+        LOW_VOLTAGE: "low voltage",
+        HIGH_CURRENT: "high current",
+        LOW_SIGNAL: "low signal",
+        OVER_POWER: "over power"
+    }
+
+    UPD_ENABLE = 0x01
+    UPD_DISABLE = 0x02
+    UPD_REQUEST = 0x03
+    UPSTREAM_UPDATE = 0x04
+
+    def __init__(self, atype=None, operation=None):
+        super(MsgHubAlert, self).__init__()
+        self.atype = atype
+        self.operation = operation
+        self.status = None
+
+    def bytes(self):
+        self.payload = pack("<B", self.atype) + pack("<B", self.operation)
+        if self.operation == self.UPD_REQUEST:
+            self.needs_reply = True
+        return super(MsgHubAlert, self).bytes()
+
+    @classmethod
+    def decode(cls, data):
+        msg = super(MsgHubAlert, cls).decode(data)
+        assert isinstance(msg, MsgHubAlert)
+        msg.atype = msg._byte()
+        msg.operation = msg._byte()
+        msg.status = msg._byte()
+
+        assert msg.operation == cls.UPSTREAM_UPDATE
+        return msg
+
+    def is_ok(self):
+        return not self.status
+
+    def is_reply(self, msg):
+        return isinstance(msg, MsgHubAlert) \
+               and msg.operation == self.UPSTREAM_UPDATE and msg.atype == self.atype
+
+
+class MsgHubAttachedIO(UpstreamMsg):
+    """
+    https://lego.github.io/lego-ble-wireless-protocol-docs/index.html#hub-attached-i-o
+    """
+    TYPE = 0x04
+
+    EVENT_DETACHED = 0x00
+    EVENT_ATTACHED = 0x01
+    EVENT_ATTACHED_VIRTUAL = 0x02
+
+    # DEVICE TYPES
+    DEV_MOTOR = 0x0001
+    DEV_SYSTEM_TRAIN_MOTOR = 0x0002
+    DEV_BUTTON = 0x0005
+    DEV_LED_LIGHT = 0x0008
+    DEV_VOLTAGE = 0x0014
+    DEV_CURRENT = 0x0015
+    DEV_PIEZO_SOUND = 0x0016
+    DEV_RGB_LIGHT = 0x0017
+    DEV_TILT_EXTERNAL = 0x0022
+    DEV_MOTION_SENSOR = 0x0023
+    DEV_VISION_SENSOR = 0x0025
+    DEV_MOTOR_EXTERNAL_TACHO = 0x0026
+    DEV_MOTOR_INTERNAL_TACHO = 0x0027
+    DEV_TILT_INTERNAL = 0x0028
+
+    def __init__(self):
+        super(MsgHubAttachedIO, self).__init__()
+        self.port = None
+        self.event = None
+
+    @classmethod
+    def decode(cls, data):
+        msg = super(MsgHubAttachedIO, cls).decode(data)
+        assert isinstance(msg, MsgHubAttachedIO)
+        msg.port = msg._byte()
+        msg.event = msg._byte()
+        return msg
+
+
+class MsgGenericError(UpstreamMsg):
+    """
+    https://lego.github.io/lego-ble-wireless-protocol-docs/index.html#generic-error-messages
+    """
+    TYPE = 0x05
+
+    ERR_ACK = 0x01  # ACK
+    ERR_MACK = 0x02  # MACK
+    ERR_BUFFER_OVERFLOW = 0x03  # Buffer Overflow
+    ERR_TIMEOUT = 0x04  # Timeout
+    ERR_WRONG_COMMAND = 0x05  # Command NOT recognized
+    ERR_WRONG_PARAMS = 0x06  # Invalid use (e.g. parameter error(s)
+    ERR_OVERCURRENT = 0x07
+    ERR_INTERNAL = 0x08
+
+    DESCR = {
+        ERR_ACK: "ACK",
+        ERR_MACK: "MACK",
+        ERR_BUFFER_OVERFLOW: "Buffer Overflow",
+        ERR_TIMEOUT: "Timeout",
+        ERR_WRONG_COMMAND: "Command NOT recognized",
+        ERR_WRONG_PARAMS: "Invalid use (e.g. parameter error(s)",
+        ERR_OVERCURRENT: "Overcurrent",
+        ERR_INTERNAL: "Internal ERROR",
+    }
+
+    def __init__(self):
+        super(MsgGenericError, self).__init__()
+        self.cmd = None
+        self.err = None
+
+    @classmethod
+    def decode(cls, data):
+        msg = super(MsgGenericError, cls).decode(data)
+        assert isinstance(msg, MsgGenericError)
+        msg.cmd = msg._byte()
+        msg.err = msg._byte()
+        return msg
+
+    def message(self):
+        return "Command 0x%x caused error 0x%x: %s" % (self.cmd, self.err, self.DESCR[self.err])
+
+
+class MsgPortInfoRequest(DownstreamMsg):
+    """
+    This is sync request for value on port
+    https://lego.github.io/lego-ble-wireless-protocol-docs/index.html#port-information-request
+    """
+    TYPE = 0x21
+
+    INFO_PORT_VALUE = 0x00
+    INFO_MODE_INFO = 0x01
+    INFO_MODE_COMBINATIONS = 0x02
+
+    def __init__(self, port, info_type):
+        super(MsgPortInfoRequest, self).__init__()
+        self.port = port
+        self.info_type = info_type
+        self.needs_reply = True
+
+    def bytes(self):
+        self.payload = pack("<B", self.port) + pack("<B", self.info_type)
+        return super(MsgPortInfoRequest, self).bytes()
+
+    def is_reply(self, msg):
+        if msg.port != self.port:
+            return False
+
+        if self.info_type == self.INFO_PORT_VALUE:
+            return isinstance(msg, (MsgPortValueSingle, MsgPortValueCombined))
+        else:
+            return isinstance(msg, (MsgPortInfo,))
+
+
+class MsgPortModeInfoRequest(DownstreamMsg):
+    """
+    https://lego.github.io/lego-ble-wireless-protocol-docs/index.html#port-mode-information-request
+    """
+    TYPE = 0x22
+
+    INFO_NAME = 0x00
+    INFO_RAW_RANGE = 0x01
+    INFO_PCT_RANGE = 0x02
+    INFO_SI_RANGE = 0x03  # no idea what 'SI' stands for
+    INFO_UNITS = 0x04
+    INFO_MAPPING = 0x05
+    # INFO_INTERNAL = 0x06
+    INFO_MOTOR_BIAS = 0x07
+    INFO_CAPABILITY_BITS = 0x08
+    INFO_VALUE_FORMAT = 0x80
+
+    INFO_TYPES = {
+        INFO_NAME: "Name",
+        INFO_RAW_RANGE: "Raw range",
+        INFO_PCT_RANGE: "Percent range",
+        INFO_SI_RANGE: "SI value range",
+        INFO_UNITS: "Units",
+        INFO_MAPPING: "Mapping",
+        INFO_MOTOR_BIAS: "Motor bias",
+        INFO_CAPABILITY_BITS: "Capabilities",
+        INFO_VALUE_FORMAT: "Value encoding",
+    }
+
+    def __init__(self, port, mode, info_type):
+        super(MsgPortModeInfoRequest, self).__init__()
+        self.port = port
+        self.mode = mode
+        self.info_type = info_type
+        self.payload = pack("<B", port) + pack("<B", mode) + pack("<B", info_type)
+        self.needs_reply = True
+
+    def is_reply(self, msg):
+        if not isinstance(msg, MsgPortModeInfo):
+            return False
+
+        if msg.port != self.port or msg.mode != self.mode or msg.info_type != self.info_type:
+            return False
+
+        return True
+
+
+class MsgPortInputFmtSetupSingle(DownstreamMsg):
+    """
+    https://lego.github.io/lego-ble-wireless-protocol-docs/index.html#port-input-format-setup-single
+    """
+    TYPE = 0x41
+
+    def __init__(self, port, mode, delta=1, update_enable=0):
+        super(MsgPortInputFmtSetupSingle, self).__init__()
+        self.port = port
+        self.mode = mode
+        self.updates_enabled = update_enable
+        self.update_delta = delta
+        self.payload = pack("<B", port) + pack("<B", mode) + pack("<I", delta) + pack("<B", update_enable)
+        self.needs_reply = True
+
+    def is_reply(self, msg):
+        if isinstance(msg, MsgPortInputFmtSingle) and msg.port == self.port:
+            return True
+
+
+class MsgPortInputFmtSetupCombined(DownstreamMsg):
+    """
+    https://lego.github.io/lego-ble-wireless-protocol-docs/index.html#port-input-format-setup-combinedmode
+    """
+    TYPE = 0x42
+
+    def __init__(self, port, mode, delta=1, update_enable=0):
+        super(MsgPortInputFmtSetupCombined, self).__init__()
+        self.port = port
+        self.payload = pack("<B", port) + pack("<B", mode) + pack("<I", delta) + pack("<B", update_enable)
+        self.needs_reply = True
+
+    def is_reply(self, msg):
+        if isinstance(msg, MsgPortInputFmtCombined) and msg.port == self.port:
+            return True
+
+
+class MsgPortInfo(UpstreamMsg):
+    """
+    https://lego.github.io/lego-ble-wireless-protocol-docs/index.html#port-information
+    """
+    TYPE = 0x43
+
+    CAP_OUTPUT = 0b00000001
+    CAP_INPUT = 0b00000010
+    CAP_COMBINABLE = 0b00000100
+    CAP_SYNCHRONIZABLE = 0b00001000
+
+    def __init__(self):
+        super(MsgPortInfo, self).__init__()
+        self.port = None
+        self.info_type = None
+        self.capabilities = None
+        self.total_modes = None
+        self.input_modes = None
+        self.output_modes = None
+        self.possible_mode_combinations = []
+
+    @classmethod
+    def decode(cls, data):
+        msg = super(MsgPortInfo, cls).decode(data)
+        assert isinstance(msg, MsgPortInfo)
+        msg.port = msg._byte()
+        msg.info_type = msg._byte()
+        if msg.info_type == MsgPortInfoRequest.INFO_MODE_INFO:
+            msg.capabilities = msg._byte()
+            msg.total_modes = msg._byte()
+            msg.input_modes = msg._bits_list(msg._short())
+            msg.output_modes = msg._bits_list(msg._short())
+        else:
+            while msg.payload:
+                # https://lego.github.io/lego-ble-wireless-protocol-docs/index.html#pos-m
+                val = msg._short()
+                msg.possible_mode_combinations.append(msg._bits_list(val))
+                if not val:
+                    break
+        return msg
+
+    def is_output(self):
+        assert self.info_type == MsgPortInfoRequest.INFO_MODE_INFO
+        return bool(self.capabilities & self.CAP_OUTPUT)
+
+    def is_input(self):
+        assert self.info_type == MsgPortInfoRequest.INFO_MODE_INFO
+        return bool(self.capabilities & self.CAP_INPUT)
+
+    def is_combinable(self):
+        assert self.info_type == MsgPortInfoRequest.INFO_MODE_INFO
+        return bool(self.capabilities & self.CAP_COMBINABLE)
+
+    def is_synchronizable(self):
+        assert self.info_type == MsgPortInfoRequest.INFO_MODE_INFO
+        return bool(self.capabilities & self.CAP_SYNCHRONIZABLE)
+
+
+class MsgPortModeInfo(UpstreamMsg):
+    """
+    https://lego.github.io/lego-ble-wireless-protocol-docs/index.html#port-mode-information
+    """
+    TYPE = 0x44
+
+    MAPPING_FLAGS = {
+        7: "Supports NULL value",
+        6: "Supports Functional Mapping 2.0+",
+        5: "N/A",
+        4: "Absolute [min..max]",
+        3: "Relative [-1..1]",
+        2: "Discrete [0, 1, 2, 3]",
+        1: "N/A",
+        0: "N/A",
+    }
+
+    DATASET_TYPES = {
+        0b00: "8 bit",
+        0b01: "16 bit",
+        0b10: "32 bit",
+        0b11: "FLOAT",
+    }
+
+    def __init__(self):
+        super(MsgPortModeInfo, self).__init__()
+        self.port = None
+        self.mode = None
+        self.info_type = None  # @see MsgPortModeInfoRequest
+        self.value = None
+
+    @classmethod
+    def decode(cls, data):
+        msg = super(MsgPortModeInfo, cls).decode(data)
+        assert isinstance(msg, MsgPortModeInfo)
+        msg.port = msg._byte()
+        msg.mode = msg._byte()
+        msg.info_type = msg._byte()
+        msg.value = msg._value()
+        return msg
+
+    def _value(self):
+        info = MsgPortModeInfoRequest
+        if self.info_type == info.INFO_NAME:
+            return self.payload[:self.payload.index(b"\00")].decode('ascii')
+        elif self.info_type in (info.INFO_RAW_RANGE, info.INFO_PCT_RANGE, info.INFO_SI_RANGE):
+            return [self._float(), self._float()]
+        elif self.info_type == info.INFO_UNITS:
+            return self.payload[:self.payload.index(b"\00")].decode('ascii')
+        elif self.info_type == info.INFO_MAPPING:
+            inp = self._bits_list(self._byte())
+            outp = self._bits_list(self._byte())
+            return {
+                "input": [self.MAPPING_FLAGS[x] for x in inp],
+                "output": [self.MAPPING_FLAGS[x] for x in outp],
+            }
+        elif self.info_type == info.INFO_MOTOR_BIAS:
+            return self._byte()
+        elif self.info_type == info.INFO_VALUE_FORMAT:
+            return {
+                "datasets": self._byte(),
+                "type": self.DATASET_TYPES[self._byte()],
+                "total_figures": self._byte(),
+                "decimals": self._byte(),
+            }
+        else:
+            return self.payload  # FIXME: will probably fail here
+
+
+class MsgPortValueSingle(UpstreamMsg):
+    """
+    https://lego.github.io/lego-ble-wireless-protocol-docs/index.html#port-value-single
+    """
+    TYPE = 0x45
+
+    def __init__(self):
+        super(MsgPortValueSingle, self).__init__()
+        self.port = None
+
+    @classmethod
+    def decode(cls, data):
+        msg = super(MsgPortValueSingle, cls).decode(data)
+        assert isinstance(msg, MsgPortValueSingle)
+        msg.port = msg._byte()
+        return msg
+
+
+class MsgPortValueCombined(UpstreamMsg):
+    """
+    https://lego.github.io/lego-ble-wireless-protocol-docs/index.html#port-value-combinedmode
+    """
+    TYPE = 0x46
+
+    def __init__(self):
+        super(MsgPortValueCombined, self).__init__()
+        self.port = None
+
+    @classmethod
+    def decode(cls, data):
+        msg = super(MsgPortValueCombined, cls).decode(data)
+        assert isinstance(msg, MsgPortValueCombined)
+        msg.port = msg._byte()
+        return msg
+
+
+class MsgPortInputFmtSingle(UpstreamMsg):
+    """
+    https://lego.github.io/lego-ble-wireless-protocol-docs/index.html#port-input-format-single
+    """
+    TYPE = 0x47
+
+    def __init__(self, port=None, mode=None, upd_enabled=None, upd_delta=None):
+        super(MsgPortInputFmtSingle, self).__init__()
+        self.port = port
+        self.mode = mode
+        self.upd_delta = upd_delta
+        self.upd_enabled = upd_enabled
+
+    @classmethod
+    def decode(cls, data):
+        msg = super(MsgPortInputFmtSingle, cls).decode(data)
+        assert isinstance(msg, MsgPortInputFmtSingle)
+        msg.port = msg._byte()
+        msg.mode = msg._byte()
+        msg.upd_delta = msg._long()
+        if len(msg.payload):
+            msg.upd_enabled = msg._byte()
+
+        return msg
+
+
+class MsgPortInputFmtCombined(UpstreamMsg):  # TODO
+    """
+    https://lego.github.io/lego-ble-wireless-protocol-docs/index.html#port-input-format-combinedmode
+    """
+    TYPE = 0x48
+
+    def __init__(self):
+        super(MsgPortInputFmtCombined, self).__init__()
+        self.port = None
+        self.combined_control = None
+
+    @classmethod
+    def decode(cls, data):
+        msg = super(MsgPortInputFmtCombined, cls).decode(data)
+        assert isinstance(msg, MsgPortInputFmtSingle)
+        msg.port = msg._byte()
+        return msg
+
+
+class MsgVirtualPortSetup(DownstreamMsg):
+    """
+    https://lego.github.io/lego-ble-wireless-protocol-docs/index.html#virtual-port-setup
+    """
+    TYPE = 0x61
+
+    CMD_DISCONNECT = 0x00
+    CMD_CONNECT = 0x01
+
+    def __init__(self, cmd, port):
+        super(MsgVirtualPortSetup, self).__init__()
+        self.payload = pack("<B", cmd)
+        if cmd == self.CMD_DISCONNECT:
+            assert isinstance(port, int)
+            self.payload += pack("<B", port)
+        else:
+            assert isinstance(port, (list, tuple))
+            self.payload += pack("<B", port[0]) + pack("<B", port[1])
+
+
+class MsgPortOutput(DownstreamMsg):
+    """
+    https://lego.github.io/lego-ble-wireless-protocol-docs/index.html#port-output-command
+    """
+    TYPE = 0x81
+
+    SC_NO_BUFFER = 0b00000001
+    SC_FEEDBACK = 0b00010000
+
+    WRITE_DIRECT = 0x50
+    WRITE_DIRECT_MODE_DATA = 0x51
+
+    def __init__(self, port, subcommand, params):
+        super(MsgPortOutput, self).__init__()
+        self.port = port
+        self.is_buffered = False
+        self.do_feedback = True
+        self.subcommand = subcommand
+        self.params = params
+
+    def bytes(self):
+        startup_completion_flags = 0
+        if not self.is_buffered:
+            startup_completion_flags |= self.SC_NO_BUFFER
+
+        if self.do_feedback:
+            startup_completion_flags |= self.SC_FEEDBACK
+            self.needs_reply = True
+
+        self.payload = pack("<B", self.port) + pack("<B", startup_completion_flags) \
+                       + pack("<B", self.subcommand) + self.params
+        return super(MsgPortOutput, self).bytes()
+
+    def is_reply(self, msg):
+        return isinstance(msg, MsgPortOutputFeedback) and msg.port == self.port \
+               and (msg.is_completed() or self.is_buffered)
+
+
+class MsgPortOutputFeedback(UpstreamMsg):
+    TYPE = 0x82
+
+    def __init__(self):
+        super(MsgPortOutputFeedback, self).__init__()
+        self.port = None
+        self.status = None
+
+    @classmethod
+    def decode(cls, data):
+        msg = super(MsgPortOutputFeedback, cls).decode(data)
+        assert isinstance(msg, MsgPortOutputFeedback)
+        assert len(msg.payload) == 2, "TODO: implement multi-port feedback message"
+        msg.port = msg._byte()
+        msg.status = msg._byte()
+        return msg
+
+    def is_in_progress(self):
+        return self.status & 0b0001
+
+    def is_completed(self):
+        return self.status & 0b0010
+
+    def is_discarded(self):
+        return self.status & 0b0100
+
+    def is_idle(self):
+        return self.status & 0b1000
+
+
+UPSTREAM_MSGS = (
+    MsgHubProperties, MsgHubAction, MsgHubAlert, MsgHubAttachedIO, MsgGenericError,
+    MsgPortInfo, MsgPortModeInfo,
+    MsgPortValueSingle, MsgPortValueCombined, MsgPortInputFmtSingle, MsgPortInputFmtCombined,
+    MsgPortOutputFeedback
+)

pylgbst/movehub.py

@@ -1,246 +0,0 @@
-import logging
-import time
-from struct import pack
-
-from pylgbst import get_connection_auto
-from pylgbst.constants import *
-from pylgbst.peripherals import Button, EncodedMotor, ColorDistanceSensor, LED, TiltSensor, Voltage, Peripheral, \
-    Amperage
-from pylgbst.utilities import str2hex, usbyte
-
-log = logging.getLogger('movehub')
-
-ENABLE_NOTIFICATIONS_HANDLE = 0x000f
-ENABLE_NOTIFICATIONS_VALUE = b'\x01\x00'
-
-
-class MoveHub(object):
-    """
-    :type connection: pylgbst.comms.Connection
-    :type devices: dict[int,Peripheral]
-    :type led: LED
-    :type tilt_sensor: TiltSensor
-    :type button: Button
-    :type amperage: Amperage
-    :type voltage: Voltage
-    :type color_distance_sensor: pylgbst.peripherals.ColorDistanceSensor
-    :type port_C: Peripheral
-    :type port_D: Peripheral
-    :type motor_A: EncodedMotor
-    :type motor_B: EncodedMotor
-    :type motor_AB: EncodedMotor
-    :type motor_external: EncodedMotor
-    """
-
-    DEV_STATUS_DETACHED = 0x00
-    DEV_STATUS_DEVICE = 0x01
-    DEV_STATUS_GROUP = 0x02
-
-    def __init__(self, connection=None):
-        if not connection:
-            connection = get_connection_auto()
-
-        self.connection = connection
-        self.info = {}
-        self.devices = {}
-
-        # shorthand fields
-        self.button = Button(self)
-        self.led = None
-        self.amperage = None
-        self.voltage = None
-        self.motor_A = None
-        self.motor_B = None
-        self.motor_AB = None
-        self.color_distance_sensor = None
-        self.tilt_sensor = None
-        self.motor_external = None
-        self.port_C = None
-        self.port_D = None
-
-        self.connection.set_notify_handler(self._notify)
-
-        self._wait_for_devices()
-        self._report_status()
-
-    def send(self, msg_type, payload):
-        cmd = pack("<B", PACKET_VER) + pack("<B", msg_type) + payload
-        self.connection.write(MOVE_HUB_HARDWARE_HANDLE, pack("<B", len(cmd) + 1) + cmd)
-
-    def _wait_for_devices(self):
-        self.connection.enable_notifications()
-
-        builtin_devices = ()
-        for num in range(0, 60):
-            builtin_devices = (self.led, self.motor_A, self.motor_B,
-                               self.motor_AB, self.tilt_sensor, self.button, self.amperage, self.voltage)
-            if None not in builtin_devices:
-                log.debug("All devices are present")
-                return
-            log.debug("Waiting for builtin devices to appear: %s", builtin_devices)
-            time.sleep(0.05)
-        log.warning("Got only these devices: %s", builtin_devices)
-        raise RuntimeError("Failed to obtain all builtin devices")
-
-    def _notify(self, handle, data):
-        orig = data
-
-        if handle != MOVE_HUB_HARDWARE_HANDLE:
-            log.warning("Unsupported notification handle: 0x%s", handle)
-            return
-
-        log.debug("Notification on %s: %s", handle, str2hex(orig))
-
-        msg_type = usbyte(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)
-        elif msg_type == MSG_SENSOR_SUBSCRIBE_ACK:
-            port = usbyte(data, 3)
-            log.debug("Sensor subscribe ack on port %s", PORTS[port])
-            self.devices[port].finished()
-        elif msg_type == MSG_PORT_CMD_ERROR:
-            log.warning("Command error: %s", str2hex(data[3:]))
-            port = usbyte(data, 3)
-            self.devices[port].finished()
-        elif msg_type == MSG_DEVICE_SHUTDOWN:
-            log.warning("Device reported shutdown: %s", str2hex(data))
-            raise KeyboardInterrupt("Device shutdown")
-        elif msg_type == MSG_DEVICE_INFO:
-            self._handle_device_info(data)
-        else:
-            log.warning("Unhandled msg type 0x%x: %s", msg_type, str2hex(orig))
-
-    def _handle_device_info(self, data):
-        kind = usbyte(data, 3)
-        if kind == 2:
-            self.button.handle_port_data(data)
-
-        if usbyte(data, 4) == 0x06:
-            self.info[kind] = data[5:]
-        else:
-            log.warning("Unhandled device info: %s", str2hex(data))
-
-    def _handle_sensor_data(self, data):
-        port = usbyte(data, 3)
-        if port not in self.devices:
-            log.warning("Notification on port with no device: %s", PORTS[port])
-            return
-
-        device = self.devices[port]
-        device.queue_port_data(data)
-
-    def _handle_port_status(self, data):
-        port = usbyte(data, 3)
-        status = usbyte(data, 4)
-
-        if status == STATUS_STARTED:
-            self.devices[port].started()
-        elif status == STATUS_FINISHED:
-            self.devices[port].finished()
-        elif status == STATUS_CONFLICT:
-            log.warning("Command conflict on port %s", PORTS[port])
-            self.devices[port].finished()
-        elif status == STATUS_INPROGRESS:
-            log.warning("Another command is in progress on port %s", PORTS[port])
-            self.devices[port].finished()
-        elif status == STATUS_INTERRUPTED:
-            log.warning("Command interrupted on port %s", PORTS[port])
-            self.devices[port].finished()
-        else:
-            log.warning("Unhandled status value: 0x%x on port %s", status, PORTS[port])
-
-    def _handle_port_info(self, data):
-        port = usbyte(data, 3)
-        status = usbyte(data, 4)
-
-        if status == self.DEV_STATUS_DETACHED:
-            log.info("Detached %s", self.devices[port])
-            self.devices[port] = None
-        elif status == self.DEV_STATUS_DEVICE or status == self.DEV_STATUS_GROUP:
-            dev_type = usbyte(data, 5)
-            self._attach_device(dev_type, port)
-        else:
-            raise ValueError("Unhandled device status: %s", status)
-
-        self._update_field(port)
-        if self.devices[port] is None:
-            del self.devices[port]
-
-    def _attach_device(self, dev_type, port):
-        if port in PORTS and dev_type in DEVICE_TYPES:
-            log.info("Attached %s on port %s", DEVICE_TYPES[dev_type], PORTS[port])
-        else:
-            log.warning("Attached device 0x%x on port 0x%x", dev_type, port)
-
-        if dev_type == DEV_MOTOR:
-            self.devices[port] = EncodedMotor(self, port)
-        elif dev_type == DEV_IMOTOR:
-            self.motor_external = EncodedMotor(self, port)
-            self.devices[port] = self.motor_external
-        elif dev_type == DEV_DCS:
-            self.color_distance_sensor = ColorDistanceSensor(self, port)
-            self.devices[port] = self.color_distance_sensor
-        elif dev_type == DEV_LED:
-            self.devices[port] = LED(self, port)
-        elif dev_type == DEV_TILT_SENSOR:
-            self.devices[port] = TiltSensor(self, port)
-        elif dev_type == DEV_AMPERAGE:
-            self.devices[port] = Amperage(self, port)
-        elif dev_type == DEV_VOLTAGE:
-            self.devices[port] = Voltage(self, port)
-        else:
-            log.warning("Unhandled peripheral type 0x%x on port 0x%x", dev_type, port)
-            self.devices[port] = Peripheral(self, port)
-
-    def _update_field(self, port):
-        if port == PORT_A:
-            self.motor_A = self.devices[port]
-        elif port == PORT_B:
-            self.motor_B = self.devices[port]
-        elif port == PORT_AB:
-            self.motor_AB = self.devices[port]
-        elif port == PORT_C:
-            self.port_C = self.devices[port]
-        elif port == PORT_D:
-            self.port_D = self.devices[port]
-        elif port == PORT_LED:
-            self.led = self.devices[port]
-        elif port == PORT_TILT_SENSOR:
-            self.tilt_sensor = self.devices[port]
-        elif port == PORT_AMPERAGE:
-            self.amperage = self.devices[port]
-        elif port == PORT_VOLTAGE:
-            self.voltage = self.devices[port]
-        else:
-            log.warning("Unhandled port: %s", PORTS[port])
-
-    def shutdown(self):
-        self.send(MSG_DEVICE_SHUTDOWN, b'')
-
-    def _report_status(self):
-        # TODO: add firmware version
-        log.info("%s by %s", self.info_get(INFO_DEVICE_NAME), self.info_get(INFO_MANUFACTURER))
-
-        self.__voltage = 0
-
-        def on_voltage(value):
-            self.__voltage = value
-
-        self.voltage.subscribe(on_voltage, granularity=0)
-        while not self.__voltage:
-            time.sleep(0.05)
-        self.voltage.unsubscribe(on_voltage)
-        log.info("Voltage: %d%%", self.__voltage * 100)
-
-    def info_get(self, info_type):
-        self.info[info_type] = None
-        self.send(MSG_DEVICE_INFO, pack("<B", info_type) + pack("<B", INFO_ACTION_GET))
-        while self.info[info_type] is None:  # FIXME: will hang forever on error
-            time.sleep(0.05)
-
-        return self.info[info_type]

pylgbst/peripherals.py

@@ -1,35 +1,69 @@
 import logging
 import math
-import time
 import traceback
 from struct import pack, unpack
 from threading import Thread
 
-from pylgbst.constants import PORTS, MSG_SENSOR_SUBSCRIBE, COLOR_NONE, COLOR_BLACK, COLORS, MSG_SET_PORT_VAL, PORT_AB, \
-    MSG_DEVICE_INFO, INFO_BUTTON_STATE, INFO_ACTION_SUBSCRIBE, INFO_ACTION_UNSUBSCRIBE
-from pylgbst.utilities import queue, str2hex, usbyte, ushort
+from pylgbst.messages import MsgHubProperties, MsgPortOutput, MsgPortInputFmtSetupSingle, MsgPortInfoRequest, \
+    MsgPortModeInfoRequest, MsgPortInfo, MsgPortModeInfo, MsgPortInputFmtSingle
+from pylgbst.utilities import queue, str2hex, usbyte, ushort, usint
 
 log = logging.getLogger('peripherals')
 
+# COLORS
+COLOR_BLACK = 0x00
+COLOR_PINK = 0x01
+COLOR_PURPLE = 0x02
+COLOR_BLUE = 0x03
+COLOR_LIGHTBLUE = 0x04
+COLOR_CYAN = 0x05
+COLOR_GREEN = 0x06
+COLOR_YELLOW = 0x07
+COLOR_ORANGE = 0x08
+COLOR_RED = 0x09
+COLOR_WHITE = 0x0a
+COLOR_NONE = 0xFF
+COLORS = {
+    COLOR_BLACK: "BLACK",
+    COLOR_PINK: "PINK",
+    COLOR_PURPLE: "PURPLE",
+    COLOR_BLUE: "BLUE",
+    COLOR_LIGHTBLUE: "LIGHTBLUE",
+    COLOR_CYAN: "CYAN",
+    COLOR_GREEN: "GREEN",
+    COLOR_YELLOW: "YELLOW",
+    COLOR_ORANGE: "ORANGE",
+    COLOR_RED: "RED",
+    COLOR_WHITE: "WHITE",
+    COLOR_NONE: "NONE"
+}
+
+
+# TODO: support more types of peripherals from
+# https://lego.github.io/lego-ble-wireless-protocol-docs/index.html#io-type-id
 
 class Peripheral(object):
     """
-    :type parent: pylgbst.movehub.MoveHub
+    :type parent: pylgbst.hub.Hub
     :type _incoming_port_data: queue.Queue
+    :type _port_mode: MsgPortInputFmtSingle
     """
 
     def __init__(self, parent, port):
         """
-        :type parent: pylgbst.movehub.MoveHub
+        :type parent: pylgbst.hub.Hub
         :type port: int
         """
         super(Peripheral, self).__init__()
-        self.parent = parent
+        self.virtual_ports = ()
+        self.hub = parent
         self.port = port
-        self._working = False
+
+        self.is_buffered = False
+
         self._subscribers = set()
-        self._port_subscription_mode = None
-        # TODO: maybe max queue len of 2?
+        self._port_mode = MsgPortInputFmtSingle(self.port, None, False, 1)
+
         self._incoming_port_data = queue.Queue(1)  # limit 1 means we drop data if we can't handle it fast enough
         thr = Thread(target=self._queue_reader)
         thr.setDaemon(True)
@@ -37,140 +71,194 @@ class Peripheral(object):
         thr.start()
 
     def __repr__(self):
-        return "%s on port %s" % (self.__class__.__name__, PORTS[self.port] if self.port in PORTS else self.port)
+        msg = "%s on port 0x%x" % (self.__class__.__name__, self.port)
+        if self.virtual_ports:
+            msg += " (ports 0x%x and 0x%x combined)" % (self.virtual_ports[0], self.virtual_ports[1])
+        return msg
 
-    def _write_to_hub(self, msg_type, params):
-        cmd = pack("<B", self.port) + params
-        self.parent.send(msg_type, cmd)
+    def set_port_mode(self, mode, send_updates=None, update_delta=None):
+        assert not self.virtual_ports, "TODO: support combined mode for sensors"
 
-    def _port_subscribe(self, mode, granularity, enable):
-        params = pack("<B", mode)
-        params += pack("<H", granularity)
-        params += b'\x00\x00'  # maybe also bytes of granularity
-        params += pack("<?", bool(enable))
-        self._write_to_hub(MSG_SENSOR_SUBSCRIBE, params)
+        if send_updates is None:
+            send_updates = self._port_mode.upd_enabled
+            log.debug("Implied update is enabled=%s", send_updates)
 
-    def started(self):
-        log.debug("Peripheral Started: %s", self)
-        self._working = True
+        if update_delta is None:
+            update_delta = self._port_mode.upd_delta
+            log.debug("Implied update delta=%s", update_delta)
 
-    def finished(self):
-        log.debug("Peripheral Finished: %s", self)
-        self._working = False
+        if self._port_mode.mode == mode \
+                and self._port_mode.upd_enabled == send_updates \
+                and self._port_mode.upd_delta == update_delta:
+            log.debug("Already in target mode, no need to switch")
+            return
+        else:
+            msg = MsgPortInputFmtSetupSingle(self.port, mode, update_delta, send_updates)
+            resp = self.hub.send(msg)
+            assert isinstance(resp, MsgPortInputFmtSingle)
+            self._port_mode = resp
 
-    def in_progress(self):
-        return bool(self._working)
+    def _send_output(self, msg):
+        assert isinstance(msg, MsgPortOutput)
+        msg.is_buffered = self.is_buffered  # TODO: support buffering
+        self.hub.send(msg)
 
-    def subscribe(self, callback, mode, granularity=1, async=False):
-        self._port_subscription_mode = mode
-        self.started()
-        self._port_subscribe(self._port_subscription_mode, granularity, True)
-
-        self._wait_sync(async)  # having async=True leads to stuck notifications
+    def get_sensor_data(self, mode):
+        self.set_port_mode(mode)
+        msg = MsgPortInfoRequest(self.port, MsgPortInfoRequest.INFO_PORT_VALUE)
+        resp = self.hub.send(msg)
+        return self._decode_port_data(resp)
 
+    def subscribe(self, callback, mode=0x00, granularity=1):
+        if self._port_mode.mode != mode and self._subscribers:
+            raise ValueError("Port is in active mode %r, unsubscribe all subscribers first" % self._port_mode)
+        self.set_port_mode(mode, True, granularity)
         if callback:
             self._subscribers.add(callback)
 
-    def unsubscribe(self, callback=None, async=False):
+    def unsubscribe(self, callback=None):
         if callback in self._subscribers:
             self._subscribers.remove(callback)
 
-        if self._port_subscription_mode is None:
-            log.warning("Attempt to unsubscribe while never subscribed: %s", self)
+        if not self._port_mode.upd_enabled:
+            log.warning("Attempt to unsubscribe while port value updates are off: %s", self)
         elif not self._subscribers:
-            self.started()
-            self._port_subscribe(self._port_subscription_mode, 0, False)
-            self._wait_sync(async)
-            self._port_subscription_mode = None
+            self.set_port_mode(self._port_mode.mode, False)
 
     def _notify_subscribers(self, *args, **kwargs):
-        for subscriber in self._subscribers:
+        for subscriber in self._subscribers.copy():
             subscriber(*args, **kwargs)
+        return args
 
-    def queue_port_data(self, data):
+    def queue_port_data(self, msg):
         try:
-            self._incoming_port_data.put_nowait(data)
+            self._incoming_port_data.put_nowait(msg)
         except queue.Full:
-            log.debug("Dropped port data: %s", data)
+            log.debug("Dropped port data: %r", msg)
 
-    def handle_port_data(self, data):
-        log.warning("Unhandled device notification for %s: %s", self, str2hex(data[4:]))
-        self._notify_subscribers(data[4:])
+    def _decode_port_data(self, msg):
+        """
+        :rtype: tuple
+        """
+        log.warning("Unhandled port data: %r", msg)
+        return ()
+
+    def _handle_port_data(self, msg):
+        """
+        :type msg: pylgbst.messages.MsgPortValueSingle
+        """
+        decoded = self._decode_port_data(msg)
+        assert isinstance(decoded, (tuple, list)), "Unexpected data type: %s" % type(decoded)
+        self._notify_subscribers(*decoded)
 
     def _queue_reader(self):
         while True:
-            data = self._incoming_port_data.get()
+            msg = self._incoming_port_data.get()
             try:
-                self.handle_port_data(data)
+                self._handle_port_data(msg)
             except BaseException:
                 log.warning("%s", traceback.format_exc())
-                log.warning("Failed to handle port data by %s: %s", self, str2hex(data))
+                log.warning("Failed to handle port data by %s: %r", self, msg)
 
-    def _wait_sync(self, async):
-        if not async:
-            log.debug("Waiting for sync command work to finish...")
-            while self.in_progress():
-                if not self.parent.connection.is_alive():
-                    log.debug("Connection is not alive anymore: %s", self.parent.connection)
+    def describe_possible_modes(self):
+        mode_info = self.hub.send(MsgPortInfoRequest(self.port, MsgPortInfoRequest.INFO_MODE_INFO))
+        assert isinstance(mode_info, MsgPortInfo)
+        info = {
+            "mode_count": mode_info.total_modes,
+            "input_modes": [],
+            "output_modes": [],
+            "capabilities": {
+                "logically_combinable": mode_info.is_combinable(),
+                "synchronizable": mode_info.is_synchronizable(),
+                "can_output": mode_info.is_output(),
+                "can_input": mode_info.is_input(),
+            }
+        }
+
+        if mode_info.is_combinable():
+            mode_combinations = self.hub.send(MsgPortInfoRequest(self.port, MsgPortInfoRequest.INFO_MODE_COMBINATIONS))
+            assert isinstance(mode_combinations, MsgPortInfo)
+            info['possible_mode_combinations'] = mode_combinations.possible_mode_combinations
+
+        info['modes'] = []
+        for mode in range(256):
+            info['modes'].append(self._describe_mode(mode))
+
+        for mode in mode_info.output_modes:
+            info['output_modes'].append(self._describe_mode(mode))
+
+        for mode in mode_info.input_modes:
+            info['input_modes'].append(self._describe_mode(mode))
+
+        log.debug("Port info for 0x%x: %s", self.port, info)
+        return info
+
+    def _describe_mode(self, mode):
+        descr = {"Mode": mode}
+        for info in MsgPortModeInfoRequest.INFO_TYPES:
+            try:
+                resp = self.hub.send(MsgPortModeInfoRequest(self.port, mode, info))
+                assert isinstance(resp, MsgPortModeInfo)
+                descr[MsgPortModeInfoRequest.INFO_TYPES[info]] = resp.value
+            except RuntimeError:
+                log.debug("Got error while requesting info 0x%x: %s", info, traceback.format_exc())
+                if info == MsgPortModeInfoRequest.INFO_NAME:
                     break
-                time.sleep(0.001)
-            log.debug("Command has finished.")
+        return descr
 
 
-class LED(Peripheral):
-    SOMETHING = b'\x51\x00'
+class LEDRGB(Peripheral):
+    MODE_INDEX = 0x00
+    MODE_RGB = 0x01
 
     def __init__(self, parent, port):
-        super(LED, self).__init__(parent, port)
-        self.last_color_set = COLOR_NONE
+        super(LEDRGB, self).__init__(parent, port)
 
-    def set_color(self, color, do_notify=True):
-        if color == COLOR_NONE:
-            color = COLOR_BLACK
+    def set_color(self, color):
+        if isinstance(color, (list, tuple)):
+            assert len(color) == 3, "RGB color has to have 3 values"
+            self.set_port_mode(self.MODE_RGB)
+            payload = pack("<B", self.MODE_RGB) + pack("<B", color[0]) + pack("<B", color[1]) + pack("<B", color[2])
+        else:
+            if color == COLOR_NONE:
+                color = COLOR_BLACK
 
-        if color not in COLORS:
-            raise ValueError("Color %s is not in list of available colors" % color)
+            if color not in COLORS:
+                raise ValueError("Color %s is not in list of available colors" % color)
 
-        self.last_color_set = color
-        cmd = pack("<B", do_notify) + self.SOMETHING + pack("<B", color)
-        self.started()
-        self._write_to_hub(MSG_SET_PORT_VAL, cmd)
-        self._wait_sync(False)
+            self.set_port_mode(self.MODE_INDEX)
+            payload = pack("<B", self.MODE_INDEX) + pack("<B", color)
 
-    def finished(self):
-        super(LED, self).finished()
-        log.debug("LED has changed color to %s", COLORS[self.last_color_set])
-        self._notify_subscribers(self.last_color_set)
+        msg = MsgPortOutput(self.port, MsgPortOutput.WRITE_DIRECT_MODE_DATA, payload)
+        self._send_output(msg)
 
-    def subscribe(self, callback, mode=None, granularity=None, async=False):
-        self._subscribers.add(callback)
-
-    def unsubscribe(self, callback=None, async=False):
-        if callback in self._subscribers:
-            self._subscribers.remove(callback)
+    def _decode_port_data(self, msg):
+        if len(msg.payload) == 3:
+            return usbyte(msg.payload, 0), usbyte(msg.payload, 1), usbyte(msg.payload, 2),
+        else:
+            return usbyte(msg.payload, 0),
 
 
-class EncodedMotor(Peripheral):
-    TRAILER = b'\x64\x7f\x03'  # NOTE: \x64 is 100, might mean something; also trailer might be a sequence terminator
-    # TODO: investigate sequence behavior, seen with zero values passed to angled mode
-    # trailer is not required for all movement types
-    MOVEMENT_TYPE = 0x11
+class Motor(Peripheral):
+    SUBCMD_START_POWER = 0x01
+    SUBCMD_START_POWER_GROUPED = 0x02
+    SUBCMD_SET_ACC_TIME = 0x05
+    SUBCMD_SET_DEC_TIME = 0x06
+    SUBCMD_START_SPEED = 0x07
+    # SUBCMD_START_SPEED = 0x08
+    SUBCMD_START_SPEED_FOR_TIME = 0x09
+    # SUBCMD_START_SPEED_FOR_TIME = 0x0A
 
-    CONSTANT_SINGLE = 0x01
-    CONSTANT_GROUP = 0x02
-    SOME_SINGLE = 0x07
-    SOME_GROUP = 0x08
-    TIMED_SINGLE = 0x09
-    TIMED_GROUP = 0x0A
-    ANGLED_SINGLE = 0x0B
-    ANGLED_GROUP = 0x0C
-
-    # MOTORS
-    SENSOR_SOMETHING1 = 0x00  # TODO: understand it
-    SENSOR_SPEED = 0x01
-    SENSOR_ANGLE = 0x02
+    END_STATE_BRAKE = 127
+    END_STATE_HOLD = 126
+    END_STATE_FLOAT = 0
 
     def _speed_abs(self, relative):
+        if relative == Motor.END_STATE_BRAKE or relative == Motor.END_STATE_HOLD:
+            # special value for BRAKE
+            # https://lego.github.io/lego-ble-wireless-protocol-docs/index.html#output-sub-command-startpower-power
+            return relative
+
         if relative < -1:
             log.warning("Speed cannot be less than -1")
             relative = -1
@@ -182,95 +270,198 @@ class EncodedMotor(Peripheral):
         absolute = math.ceil(relative * 100)  # scale of 100 is proven by experiments
         return int(absolute)
 
-    def _wrap_and_write(self, mtype, params, speed_primary, speed_secondary):
-        if self.port == PORT_AB:
-            mtype += 1  # de-facto rule
+    def _write_direct_mode(self, subcmd, params):
+        params = pack("<B", subcmd) + params
+        msg = MsgPortOutput(self.port, MsgPortOutput.WRITE_DIRECT_MODE_DATA, params)
+        self._send_output(msg)
 
-        abs_primary = self._speed_abs(speed_primary)
-        abs_secondary = self._speed_abs(speed_secondary)
+    def _send_cmd(self, subcmd, params):
+        if self.virtual_ports:
+            subcmd += 1  # de-facto rule
 
-        if mtype == self.ANGLED_GROUP and (not abs_secondary or not abs_primary):
-            raise ValueError("Cannot have zero speed in double angled mode")  # otherwise it gets nuts
+        msg = MsgPortOutput(self.port, subcmd, params)
+        self._send_output(msg)
 
-        params = pack("<B", self.MOVEMENT_TYPE) + pack("<B", mtype) + params
+    def start_power(self, power_primary=1.0, power_secondary=None):
+        """
+        https://lego.github.io/lego-ble-wireless-protocol-docs/index.html#output-sub-command-startpower-power
+        """
+        if power_secondary is None:
+            power_secondary = power_primary
 
-        params += pack("<b", abs_primary)
-        if self.port == PORT_AB:
-            params += pack("<b", abs_secondary)
+        if self.virtual_ports:
+            cmd = self.SUBCMD_START_POWER_GROUPED - 1  # because _send_cmd will do +1
+        else:
+            cmd = self.SUBCMD_START_POWER
 
-        params += self.TRAILER
+        params = b""
+        params += pack("<b", self._speed_abs(power_primary))
+        if self.virtual_ports:
+            params += pack("<b", self._speed_abs(power_secondary))
 
-        self._write_to_hub(MSG_SET_PORT_VAL, params)
+        self._send_cmd(cmd, params)
 
-    def timed(self, seconds, speed_primary=1, speed_secondary=None, async=False):
+    def stop(self):
+        self.timed(0)
+
+    def set_acc_profile(self, seconds, profile_no=0x00):
+        """
+        https://lego.github.io/lego-ble-wireless-protocol-docs/index.html#output-sub-command-setacctime-time-profileno-0x05
+        """
+        params = b""
+        params += pack("<H", int(seconds * 1000))
+        params += pack("<B", profile_no)
+
+        self._send_cmd(self.SUBCMD_SET_ACC_TIME, params)
+
+    def set_dec_profile(self, seconds, profile_no=0x00):
+        """
+        https://lego.github.io/lego-ble-wireless-protocol-docs/index.html#output-sub-command-setdectime-time-profileno-0x06
+        """
+        params = b""
+        params += pack("<H", int(seconds * 1000))
+        params += pack("<B", profile_no)
+
+        self._send_cmd(self.SUBCMD_SET_DEC_TIME, params)
+
+    def start_speed(self, speed_primary=1.0, speed_secondary=None, max_power=1.0, use_profile=0b11):
+        """
+        https://lego.github.io/lego-ble-wireless-protocol-docs/index.html#output-sub-command-startspeed-speed-maxpower-useprofile-0x07
+        """
         if speed_secondary is None:
             speed_secondary = speed_primary
 
-        params = pack('<H', int(seconds * 1000))
+        params = b""
+        params += pack("<b", self._speed_abs(speed_primary))
+        if self.virtual_ports:
+            params += pack("<b", self._speed_abs(speed_secondary))
 
-        self.started()
-        self._wrap_and_write(self.TIMED_SINGLE, params, speed_primary, speed_secondary)
-        self._wait_sync(async)
+        params += pack("<B", int(100 * max_power))
+        params += pack("<B", use_profile)
 
-    def angled(self, angle, speed_primary=1, speed_secondary=None, async=False):
+        self._send_cmd(self.SUBCMD_START_SPEED, params)
+
+    def timed(self, seconds, speed_primary=1.0, speed_secondary=None, max_power=1.0, end_state=END_STATE_BRAKE,
+              use_profile=0b11):
+        """
+        https://lego.github.io/lego-ble-wireless-protocol-docs/index.html#output-sub-command-startspeedfortime-time-speed-maxpower-endstate-useprofile-0x09
+        """
         if speed_secondary is None:
             speed_secondary = speed_primary
 
-        angle = int(round(angle))
-        if angle < 0:
-            angle = -angle
+        params = b""
+        params += pack("<H", int(seconds * 1000))
+        params += pack("<b", self._speed_abs(speed_primary))
+        if self.virtual_ports:
+            params += pack("<b", self._speed_abs(speed_secondary))
+
+        params += pack("<B", int(100 * max_power))
+        params += pack("<B", end_state)
+        params += pack("<B", use_profile)
+
+        self._send_cmd(self.SUBCMD_START_SPEED_FOR_TIME, params)
+
+
+class EncodedMotor(Motor):
+    SUBCMD_START_SPEED_FOR_DEGREES = 0x0B
+    # SUBCMD_START_SPEED_FOR_DEGREES = 0x0C
+    SUBCMD_GOTO_ABSOLUTE_POSITION = 0x0D
+    # SUBCMD_GOTO_ABSOLUTE_POSITIONC = 0x0E
+    SUBCMD_PRESET_ENCODER = 0x14
+
+    SENSOR_POWER = 0x00  # it's not input mode, hovewer returns some numbers
+    SENSOR_SPEED = 0x01
+    SENSOR_ANGLE = 0x02
+    SENSOR_TEST = 0x03  # exists, but neither input nor output mode
+
+    def angled(self, degrees, speed_primary=1.0, speed_secondary=None, max_power=1.0, end_state=Motor.END_STATE_BRAKE,
+               use_profile=0b11):
+        """
+        https://lego.github.io/lego-ble-wireless-protocol-docs/index.html#output-sub-command-startspeedfordegrees-degrees-speed-maxpower-endstate-useprofile-0x0b
+        :type degrees: int
+        :type speed_primary: float
+        """
+        if speed_secondary is None:
+            speed_secondary = speed_primary
+
+        degrees = int(round(degrees))
+        if degrees < 0:
+            degrees = -degrees
             speed_primary = -speed_primary
             speed_secondary = -speed_secondary
 
-        params = pack('<I', angle)
+        params = b""
+        params += pack("<I", degrees)
+        params += pack("<b", self._speed_abs(speed_primary))
+        if self.virtual_ports:
+            params += pack("<b", self._speed_abs(speed_secondary))
 
-        self.started()
-        self._wrap_and_write(self.ANGLED_SINGLE, params, speed_primary, speed_secondary)
-        self._wait_sync(async)
+        params += pack("<B", int(100 * max_power))
+        params += pack("<B", end_state)
+        params += pack("<B", use_profile)
 
-    def constant(self, speed_primary=1, speed_secondary=None, async=False):
-        if speed_secondary is None:
-            speed_secondary = speed_primary
+        self._send_cmd(self.SUBCMD_START_SPEED_FOR_DEGREES, params)
 
-        self.started()
-        self._wrap_and_write(self.CONSTANT_SINGLE, b"", speed_primary, speed_secondary)
-        self._wait_sync(async)
+    def goto_position(self, degrees_primary, degrees_secondary=None, speed=1.0, max_power=1.0,
+                      end_state=Motor.END_STATE_BRAKE, use_profile=0b11):
+        """
+        https://lego.github.io/lego-ble-wireless-protocol-docs/index.html#output-sub-command-gotoabsoluteposition-abspos-speed-maxpower-endstate-useprofile-0x0d
+        """
+        if degrees_secondary is None:
+            degrees_secondary = degrees_primary
 
-    def __some(self, speed_primary=1, speed_secondary=None, async=False):
-        if speed_secondary is None:
-            speed_secondary = speed_primary
+        params = b""
+        params += pack("<i", degrees_primary)
+        if self.virtual_ports:
+            params += pack("<i", degrees_secondary)
 
-        self.started()
-        self._wrap_and_write(self.SOME_SINGLE, b"", speed_primary, speed_secondary)
-        self._wait_sync(async)
+        params += pack("<b", self._speed_abs(speed))
 
-    def stop(self, async=False):
-        self.constant(0, async=async)
+        params += pack("<B", int(100 * max_power))
+        params += pack("<B", end_state)
+        params += pack("<B", use_profile)
 
-    def handle_port_data(self, data):
-        if self._port_subscription_mode == self.SENSOR_ANGLE:
-            rotation = unpack("<l", data[4:8])[0]
-            self._notify_subscribers(rotation)
-        elif self._port_subscription_mode == self.SENSOR_SOMETHING1:
-            # TODO: understand what it means
-            rotation = usbyte(data, 4)
-            self._notify_subscribers(rotation)
-        elif self._port_subscription_mode == self.SENSOR_SPEED:
-            rotation = unpack("<b", data[4])[0]
-            self._notify_subscribers(rotation)
+        self._send_cmd(self.SUBCMD_GOTO_ABSOLUTE_POSITION, params)
+
+    def _decode_port_data(self, msg):
+        data = msg.payload
+        if self._port_mode.mode == self.SENSOR_ANGLE:
+            angle = unpack("<l", data[0:4])[0]
+            return (angle,)
+        elif self._port_mode.mode == self.SENSOR_SPEED:
+            speed = unpack("<b", data[0:1])[0]
+            return (speed,)
         else:
-            log.debug("Got motor sensor data while in unexpected mode: %s", self._port_subscription_mode)
+            log.debug("Got motor sensor data while in unexpected mode: %r", self._port_mode)
+            return ()
 
-    def subscribe(self, callback, mode=SENSOR_ANGLE, granularity=1, async=False):
+    def subscribe(self, callback, mode=SENSOR_ANGLE, granularity=1):
         super(EncodedMotor, self).subscribe(callback, mode, granularity)
 
+    def preset_encoder(self, degrees=0, degrees_secondary=None, only_combined=False):
+        """
+        https://lego.github.io/lego-ble-wireless-protocol-docs/index.html#output-sub-command-presetencoder-position-n-a
+        https://lego.github.io/lego-ble-wireless-protocol-docs/index.html#output-sub-command-presetencoder-leftposition-rightposition-0x14
+        """
+        if degrees_secondary is None:
+            degrees_secondary = degrees
+
+        if self.virtual_ports and not only_combined:
+            self._send_cmd(self.SUBCMD_PRESET_ENCODER, pack("<i", degrees) + pack("<i", degrees_secondary))
+        else:
+            params = pack("<i", degrees)
+            self._write_direct_mode(self.SENSOR_ANGLE, params)
+
 
 class TiltSensor(Peripheral):
-    MODE_2AXIS_FULL = 0x00
+    MODE_2AXIS_ANGLE = 0x00
     MODE_2AXIS_SIMPLE = 0x01
     MODE_3AXIS_SIMPLE = 0x02
-    MODE_BUMP_COUNT = 0x03
-    MODE_3AXIS_FULL = 0x04
+    MODE_IMPACT_COUNT = 0x03
+    MODE_3AXIS_ACCEL = 0x04
+    MODE_ORIENT_CF = 0x05
+    MODE_IMPACT_CF = 0x06
+    MODE_CALIBRATION = 0x07
 
     TRI_BACK = 0x00
     TRI_UP = 0x01
@@ -302,134 +493,152 @@ class TiltSensor(Peripheral):
         TRI_FRONT: "FRONT",
     }
 
-    def subscribe(self, callback, mode=MODE_3AXIS_SIMPLE, granularity=1, async=False):
+    def subscribe(self, callback, mode=MODE_3AXIS_SIMPLE, granularity=1):
         super(TiltSensor, self).subscribe(callback, mode, granularity)
 
-    def handle_port_data(self, data):
-        if self._port_subscription_mode == self.MODE_3AXIS_SIMPLE:
-            state = usbyte(data, 4)
-            self._notify_subscribers(state)
-        elif self._port_subscription_mode == self.MODE_2AXIS_SIMPLE:
-            state = usbyte(data, 4)
-            self._notify_subscribers(state)
-        elif self._port_subscription_mode == self.MODE_BUMP_COUNT:
-            bump_count = ushort(data, 4)
-            self._notify_subscribers(bump_count)
-        elif self._port_subscription_mode == self.MODE_2AXIS_FULL:
-            roll = self._byte2deg(usbyte(data, 4))
-            pitch = self._byte2deg(usbyte(data, 5))
-            self._notify_subscribers(roll, pitch)
-        elif self._port_subscription_mode == self.MODE_3AXIS_FULL:
-            roll = self._byte2deg(usbyte(data, 4))
-            pitch = self._byte2deg(usbyte(data, 5))
-            yaw = self._byte2deg(usbyte(data, 6))  # did I get the order right?
-            self._notify_subscribers(roll, pitch, yaw)
+    def _decode_port_data(self, msg):
+        data = msg.payload
+        if self._port_mode.mode == self.MODE_2AXIS_ANGLE:
+            roll = unpack('<b', data[0:1])[0]
+            pitch = unpack('<b', data[1:2])[0]
+            return (roll, pitch)
+        elif self._port_mode.mode == self.MODE_3AXIS_SIMPLE:
+            state = usbyte(data, 0)
+            return (state,)
+        elif self._port_mode.mode == self.MODE_2AXIS_SIMPLE:
+            state = usbyte(data, 0)
+            return (state,)
+        elif self._port_mode.mode == self.MODE_IMPACT_COUNT:
+            bump_count = usint(data, 0)
+            return (bump_count,)
+        elif self._port_mode.mode == self.MODE_3AXIS_ACCEL:
+            roll = unpack('<b', data[0:1])[0]
+            pitch = unpack('<b', data[1:2])[0]
+            yaw = unpack('<b', data[2:3])[0]  # did I get the order right?
+            return (roll, pitch, yaw)
+        elif self._port_mode.mode == self.MODE_ORIENT_CF:
+            state = usbyte(data, 0)
+            return (state,)
+        elif self._port_mode.mode == self.MODE_IMPACT_CF:
+            state = usbyte(data, 0)
+            return (state,)
+        elif self._port_mode.mode == self.MODE_CALIBRATION:
+            return (usbyte(data, 0), usbyte(data, 1), usbyte(data, 2))
         else:
-            log.debug("Got tilt sensor data while in unexpected mode: %s", self._port_subscription_mode)
+            log.debug("Got tilt sensor data while in unexpected mode: %r", self._port_mode)
+            return ()
 
-    def _byte2deg(self, val):
-        if val > 90:
-            return val - 256
-        else:
-            return val
+    # TODO: add some methods from official doc, like
+    # https://lego.github.io/lego-ble-wireless-protocol-docs/index.html#output-sub-command-tiltconfigimpact-impactthreshold-bumpholdoff-n-a
 
 
-class ColorDistanceSensor(Peripheral):
-    COLOR_ONLY = 0x00
+class VisionSensor(Peripheral):
+    COLOR_INDEX = 0x00
     DISTANCE_INCHES = 0x01
     COUNT_2INCH = 0x02
-    DISTANCE_HOW_CLOSE = 0x03
-    DISTANCE_SUBINCH_HOW_CLOSE = 0x04
-    OFF1 = 0x05
-    STREAM_3_VALUES = 0x06
-    OFF2 = 0x07
-    COLOR_DISTANCE_FLOAT = 0x08
-    LUMINOSITY = 0x09
-    SOME_20BYTES = 0x0a  # TODO: understand it
+    DISTANCE_REFLECTED = 0x03
+    AMBIENT_LIGHT = 0x04
+    SET_COLOR = 0x05
+    COLOR_RGB = 0x06
+    SET_IR_TX = 0x07
+    COLOR_DISTANCE_FLOAT = 0x08  # it's not declared by dev's mode info
+
+    DEBUG = 0x09  # first val is by fact ambient light, second is zero
+    CALIBRATE = 0x0a  # gives constant values
 
     def __init__(self, parent, port):
-        super(ColorDistanceSensor, self).__init__(parent, port)
+        super(VisionSensor, self).__init__(parent, port)
 
-    def subscribe(self, callback, mode=COLOR_DISTANCE_FLOAT, granularity=1, async=False):
-        super(ColorDistanceSensor, self).subscribe(callback, mode, granularity)
+    def subscribe(self, callback, mode=COLOR_DISTANCE_FLOAT, granularity=1):
+        super(VisionSensor, self).subscribe(callback, mode, granularity)
 
-    def handle_port_data(self, data):
-        if self._port_subscription_mode == self.COLOR_DISTANCE_FLOAT:
-            color = usbyte(data, 4)
-            distance = usbyte(data, 5)
-            partial = usbyte(data, 7)
+    def _decode_port_data(self, msg):
+        data = msg.payload
+        if self._port_mode.mode == self.COLOR_INDEX:
+            color = usbyte(data, 0)
+            return (color,)
+        elif self._port_mode.mode == self.COLOR_DISTANCE_FLOAT:
+            color = usbyte(data, 0)
+            val = usbyte(data, 1)
+            partial = usbyte(data, 3)
             if partial:
-                distance += 1.0 / partial
-            self._notify_subscribers(color, float(distance))
-        elif self._port_subscription_mode == self.COLOR_ONLY:
-            color = usbyte(data, 4)
-            self._notify_subscribers(color)
-        elif self._port_subscription_mode == self.DISTANCE_INCHES:
-            distance = usbyte(data, 4)
-            self._notify_subscribers(distance)
-        elif self._port_subscription_mode == self.DISTANCE_HOW_CLOSE:
-            distance = usbyte(data, 4)
-            self._notify_subscribers(distance)
-        elif self._port_subscription_mode == self.DISTANCE_SUBINCH_HOW_CLOSE:
-            distance = usbyte(data, 4)
-            self._notify_subscribers(distance)
-        elif self._port_subscription_mode == self.OFF1 or self._port_subscription_mode == self.OFF2:
-            log.info("Turned off led on %s", self)
-        elif self._port_subscription_mode == self.COUNT_2INCH:
-            count = unpack("<L", data[4:8])[0]  # is it all 4 bytes or just 2?
-            self._notify_subscribers(count)
-        elif self._port_subscription_mode == self.STREAM_3_VALUES:
-            # TODO: understand better meaning of these 3 values
-            val1 = ushort(data, 4)
-            val2 = ushort(data, 6)
-            val3 = ushort(data, 8)
-            self._notify_subscribers(val1, val2, val3)
-        elif self._port_subscription_mode == self.LUMINOSITY:
-            luminosity = ushort(data, 4) / 1023.0
-            self._notify_subscribers(luminosity)
-        else:  # TODO: support whatever we forgot
-            log.debug("Unhandled data in mode %s: %s", self._port_subscription_mode, str2hex(data))
+                val += 1.0 / partial
+            return (color, float(val))
+        elif self._port_mode.mode == self.DISTANCE_INCHES:
+            val = usbyte(data, 0)
+            return (val,)
+        elif self._port_mode.mode == self.DISTANCE_REFLECTED:
+            val = usbyte(data, 0) / 100.0
+            return (val,)
+        elif self._port_mode.mode == self.AMBIENT_LIGHT:
+            val = usbyte(data, 0) / 100.0
+            return (val,)
+        elif self._port_mode.mode == self.COUNT_2INCH:
+            count = usint(data, 0)
+            return (count,)
+        elif self._port_mode.mode == self.COLOR_RGB:
+            val1 = int(255 * ushort(data, 0) / 1023.0)
+            val2 = int(255 * ushort(data, 2) / 1023.0)
+            val3 = int(255 * ushort(data, 4) / 1023.0)
+            return (val1, val2, val3)
+        elif self._port_mode.mode == self.DEBUG:
+            val1 = 10 * ushort(data, 0) / 1023.0
+            val2 = 10 * ushort(data, 2) / 1023.0
+            return (val1, val2)
+        elif self._port_mode.mode == self.CALIBRATE:
+            return [ushort(data, x * 2) for x in range(8)]
+        else:
+            log.debug("Unhandled VisionSensor data in mode %s: %s", self._port_mode.mode, str2hex(data))
+            return ()
+
+    def set_color(self, color):
+        if color == COLOR_NONE:
+            color = COLOR_BLACK
+
+        if color not in COLORS:
+            raise ValueError("Color %s is not in list of available colors" % color)
+
+        self.set_port_mode(self.SET_COLOR)
+        payload = pack("<B", self.SET_COLOR) + pack("<B", color)
+
+        msg = MsgPortOutput(self.port, MsgPortOutput.WRITE_DIRECT_MODE_DATA, payload)
+        self._send_output(msg)
+
+    def set_ir_tx(self, level=1.0):
+        assert 0 <= level <= 1.0
+        self.set_port_mode(self.SET_IR_TX)
+        payload = pack("<B", self.SET_IR_TX) + pack("<H", int(level * 65535))
+
+        msg = MsgPortOutput(self.port, MsgPortOutput.WRITE_DIRECT_MODE_DATA, payload)
+        self._send_output(msg)
 
 
 class Voltage(Peripheral):
-    MODE1 = 0x00  # give less frequent notifications
-    MODE2 = 0x01  # give more frequent notifications, maybe different value (cpu vs board?)
+    # sensor says there are "L" and "S" values, but what are they?
+    VOLTAGE_L = 0x00
+    VOLTAGE_S = 0x01
 
     def __init__(self, parent, port):
         super(Voltage, self).__init__(parent, port)
-        self.last_value = None
 
-    def subscribe(self, callback, mode=MODE1, granularity=1, async=False):
-        super(Voltage, self).subscribe(callback, mode, granularity)
-
-    # we know only voltage subscription from it. is it really battery or just onboard voltage?
-    # device has turned off on 1b0e00060045 3c 0803 / 1b0e000600453c 0703
-    # moderate 9v ~= 3840
-    # good 7.5v ~= 3892
-    # liion 5v ~= 2100
-    def handle_port_data(self, data):
-        val = ushort(data, 4)
-        self.last_value = val / 4096.0
-        if self.last_value < 0.2:
-            log.warning("Battery low! %s%%", int(100 * self.last_value))
-        self._notify_subscribers(self.last_value)
+    def _decode_port_data(self, msg):
+        data = msg.payload
+        val = ushort(data, 0)
+        volts = 9600.0 * val / 3893.0 / 1000.0
+        return (volts,)
 
 
-class Amperage(Peripheral):
-    MODE1 = 0x00  # give less frequent notifications
-    MODE2 = 0x01  # give more frequent notifications, maybe different value (cpu vs board?)
+class Current(Peripheral):
+    CURRENT_L = 0x00
+    CURRENT_S = 0x01
 
     def __init__(self, parent, port):
-        super(Amperage, self).__init__(parent, port)
-        self.last_value = None
+        super(Current, self).__init__(parent, port)
 
-    def subscribe(self, callback, mode=MODE1, granularity=1, async=False):
-        super(Amperage, self).subscribe(callback, mode, granularity)
-
-    def handle_port_data(self, data):
-        val = ushort(data, 4)
-        self.last_value = val / 4096.0
-        self._notify_subscribers(self.last_value)
+    def _decode_port_data(self, msg):
+        val = ushort(msg.payload, 0)
+        milliampers = 2444 * val / 4095.0
+        return (milliampers,)
 
 
 class Button(Peripheral):
@@ -439,25 +648,24 @@ class Button(Peripheral):
 
     def __init__(self, parent):
         super(Button, self).__init__(parent, 0)  # fake port 0
+        self.hub.add_message_handler(MsgHubProperties, self._props_msg)
 
-    def subscribe(self, callback, mode=None, granularity=1, async=False):
-        self.started()
-        self.parent.send(MSG_DEVICE_INFO, pack('<B', INFO_BUTTON_STATE) + pack('<B', INFO_ACTION_SUBSCRIBE))
-        self._wait_sync(async)
+    def subscribe(self, callback, mode=None, granularity=1):
+        self.hub.send(MsgHubProperties(MsgHubProperties.BUTTON, MsgHubProperties.UPD_ENABLE))
 
         if callback:
             self._subscribers.add(callback)
 
-    def unsubscribe(self, callback=None, async=False):
+    def unsubscribe(self, callback=None):
         if callback in self._subscribers:
             self._subscribers.remove(callback)
 
         if not self._subscribers:
-            self.parent.send(MSG_DEVICE_INFO, pack('<B', INFO_BUTTON_STATE) + pack('<B', INFO_ACTION_UNSUBSCRIBE))
-            # FIXME: will this require sync wait?
+            self.hub.send(MsgHubProperties(MsgHubProperties.BUTTON, MsgHubProperties.UPD_DISABLE))
 
-    def handle_port_data(self, data):
-        param = usbyte(data, 5)
-        if self.in_progress():
-            self.finished()
-        self._notify_subscribers(bool(param))
+    def _props_msg(self, msg):
+        """
+        :type msg: MsgHubProperties
+        """
+        if msg.property == MsgHubProperties.BUTTON and msg.operation == MsgHubProperties.UPSTREAM_UPDATE:
+            self._notify_subscribers(usbyte(msg.parameters, 0))

pylgbst/utilities.py

@@ -3,9 +3,12 @@ This module offers some utilities, in a way they are work in both Python 2 and 3
 """
 
 import binascii
+import logging
 import sys
 from struct import unpack
 
+log = logging.getLogger(__name__)
+
 if sys.version_info[0] == 2:
     import Queue as queue
 else:
@@ -14,13 +17,29 @@ else:
 queue = queue  # just to use it
 
 
+def check_unpack(seq, index, pattern, size):
+    """Check that we got size bytes, if so, unpack using pattern"""
+    data = seq[index: index + size]
+    assert len(data) == size, "Unexpected data len %d, expected %d" % (len(data), size)
+    return unpack(pattern, data)[0]
+
+
 def usbyte(seq, index):
-    return unpack("<B", seq[index:index + 1])[0]
+    return check_unpack(seq, index, "<B", 1)
 
 
 def ushort(seq, index):
-    return unpack("<H", seq[index:index + 2])[0]
+    return check_unpack(seq, index, "<H", 2)
+
+
+def usint(seq, index):
+    return check_unpack(seq, index, "<I", 4)
 
 
 def str2hex(data):  # we need it for python 2+3 compatibility
-    return binascii.hexlify(data).decode("utf8")
+    # if sys.version_info[0] == 3:
+    # data = bytes(data, 'ascii')
+    if not isinstance(data, (bytes, bytearray)):
+        data = bytes(data, "ascii")
+    hexed = binascii.hexlify(data)
+    return hexed

setup.cfg

@@ -0,0 +1,2 @@
+[metadata]
+description-file = README.md

setup.py

@@ -1,15 +1,24 @@
-from distutils.core import setup
+from setuptools import setup
 
-setup(name='pylgbst',
-      description='Python library to interact with LEGO Move Hub (from Lego BOOST set)',
-      version='0.8',
-      author='Andrey Pokhilko',
-      author_email='apc4@ya.ru',
-      packages=['pylgbst'],
-      requires=[],
-      extras_require={
-          'gatt': ["gatt"],
-          'gattlib': ["gattlib"],
-          'pygatt': ["pygatt"],
-      }
-      )
+setup(
+    name="pylgbst",
+    version="1.2.0",
+
+    author="Andrey Pokhilko",
+    author_email="apc4@ya.ru",
+    license="MIT",
+    description="Python library to interact with LEGO PoweredUp devices (Lego BOOST etc.)",
+    url='https://github.com/undera/pylgbst',
+    keywords=['LEGO', 'ROBOTICS', 'BLUETOOTH'],
+
+    packages=["pylgbst", "pylgbst.comms"],
+    requires=[],
+    extras_require={
+        # Note that dbus and gi are normally system packages
+        "gatt": ["gatt", "dbus", "gi"],
+        "gattlib": ["gattlib"],
+        "pygatt": ["pygatt", "pexpect"],
+        "bluepy": ["bluepy"],
+        "bleak": ["bleak"],
+    },
+)

tests/__init__.py

@@ -1,30 +1,28 @@
+import sys
+import time
 from binascii import unhexlify
 
 from pylgbst.comms import Connection
-from pylgbst.movehub import MoveHub
+from pylgbst.hub import MoveHub, Hub
 from pylgbst.peripherals import *
 
-logging.basicConfig(level=logging.DEBUG)
+logging.basicConfig(level=logging.DEBUG if 'pydevd' in sys.modules else logging.INFO)
 
 log = logging.getLogger('test')
 
 
-class HubMock(MoveHub):
+class HubMock(Hub):
+    """
+    :type connection: ConnectionMock
+    """
+
     # noinspection PyUnresolvedReferences
-    def __init__(self, connection=None):
-        """
-        :type connection: ConnectionMock
-        """
-        super(HubMock, self).__init__(connection if connection else ConnectionMock())
+    def __init__(self, conn=None):
+        super(HubMock, self).__init__(conn if conn else ConnectionMock())
+        self.connection = self.connection
         self.notify_mock = self.connection.notifications
         self.writes = self.connection.writes
 
-    def _wait_for_devices(self):
-        pass
-
-    def _report_status(self):
-        pass
-
 
 class ConnectionMock(Connection):
     """
@@ -39,22 +37,51 @@ class ConnectionMock(Connection):
         self.running = True
         self.finished = False
 
+        self.thr = Thread(target=self.notifier)
+        self.thr.setDaemon(True)
+
     def set_notify_handler(self, handler):
         self.notification_handler = handler
-        thr = Thread(target=self.notifier)
-        thr.setDaemon(True)
-        thr.start()
+        self.thr.start()
 
     def notifier(self):
-        while self.running:
+        while self.running or self.notifications:
             if self.notification_handler:
                 while self.notifications:
-                    handle, data = self.notifications.pop(0)
-                    self.notification_handler(handle, unhexlify(data.replace(' ', '')))
-            time.sleep(0.1)
+                    data = self.notifications.pop(0)
+                    s = unhexlify(data.replace(' ', ''))
+                    self.notification_handler(MoveHub.HUB_HARDWARE_HANDLE, bytes(s))
+            time.sleep(0.01)
 
         self.finished = True
 
     def write(self, handle, data):
         log.debug("Writing to %s: %s", handle, str2hex(data))
         self.writes.append((handle, str2hex(data)))
+
+    def connect(self, hub_mac=None):
+        """
+        :rtype: ConnectionMock
+        """
+        super(ConnectionMock, self).connect(hub_mac)
+        log.debug("Mock connected")
+        return self
+
+    def is_alive(self):
+        return not self.finished and self.thr.is_alive()
+
+    def notification_delayed(self, payload, pause=0.001):
+        def inject():
+            time.sleep(pause)
+            self.notifications.append(payload)
+
+        Thread(target=inject).start()
+
+    def wait_notifications_handled(self):
+        self.running = False
+        for _ in range(1, 180):
+            time.sleep(0.01)
+            log.debug("Waiting for notifications to process...")
+            if self.finished:
+                log.debug("Done waiting for notifications to process")
+                break

tests/test_cbleak.py

@@ -0,0 +1,61 @@
+import sys
+import time
+import unittest
+
+import bleak
+from packaging import version
+
+import pylgbst
+import pylgbst.comms.cbleak as cbleak
+
+bleak.BleakClient = object()
+bleak.discover = object()
+
+last_response = None
+lt37 = version.parse(sys.version.split(' ')[0]) < version.parse("3.7")
+
+
+class BleakDriverTest(unittest.TestCase):
+    def test_driver_creation(self):
+        connection = pylgbst.get_connection_bleak()
+        self.assertIsInstance(connection, cbleak.BleakDriver)
+        self.assertFalse(connection.is_alive(), 'Checking that factory returns not started driver')
+
+    @unittest.skipIf(lt37, "Python version is too low")
+    def test_communication(self):
+        driver = cbleak.BleakDriver()
+
+        async def fake_thread():
+            print('Fake thread initialized')
+            while not driver._abort:
+                time.sleep(0.1)
+                if cbleak.req_queue.qsize() != 0:
+                    print('Received data, sending back')
+                    data = cbleak.req_queue.get()
+                    cbleak.resp_queue.put(data)
+
+        driver._bleak_thread = fake_thread
+        driver.set_notify_handler(BleakDriverTest.validation_handler)
+        driver.enable_notifications()
+
+        time.sleep(0.5)  # time for driver initialization
+        self.assertTrue(driver.is_alive(), 'Checking that driver starts')
+        handle = 0x32
+        data = [0xD, 0xE, 0xA, 0xD, 0xB, 0xE, 0xE, 0xF]
+        driver.write(handle, data)
+        time.sleep(0.5)  # processing time
+        self.assertEqual(handle, last_response[0], 'Verifying response handle')
+        self.assertEqual(bytes(data), last_response[1], 'Verifying response data')
+
+        driver.disconnect()
+        time.sleep(0.5)  # processing time
+        self.assertFalse(driver.is_alive())
+
+    @staticmethod
+    def validation_handler(handle, data):
+        global last_response
+        last_response = (handle, data)
+
+
+if __name__ == '__main__':
+    unittest.main()

tests/test_cbluepy.py

@@ -0,0 +1,61 @@
+import unittest
+
+import pylgbst.comms.cbluepy as bp_backend
+
+
+class PeripheralMock(object):
+    def __init__(self, addr, addrType, ifaceNumber):
+        pass
+
+    def waitForNotifications(self, timeout):
+        pass
+
+    def writeCharacteristic(self, handle, data):
+        pass
+
+    def withDelegate(self, delegate):
+        pass
+
+    def disconnect(self):
+        pass
+
+
+bp_backend.PROPAGATE_DISPATCHER_EXCEPTION = True
+bp_backend.btle.Peripheral = lambda *args, **kwargs: PeripheralMock(*args, **kwargs)
+
+
+class BluepyTestCase(unittest.TestCase):
+    def test_get_iface_number(self):
+        self.assertEqual(bp_backend._get_iface_number('hci0'), 0)
+        self.assertEqual(bp_backend._get_iface_number('hci10'), 10)
+        try:
+            bp_backend._get_iface_number('ads12')
+            self.fail('Missing exception for incorrect value')
+        except ValueError:
+            pass
+
+    def test_delegate(self):
+        def _handler(handle, data):
+            _handler.called = True
+
+        delegate = bp_backend.BluepyDelegate(_handler)
+        delegate.handleNotification(123, 'qwe')
+        self.assertEqual(_handler.called, True)
+
+    def test_threaded_peripheral(self):
+        tp = bp_backend.BluepyThreadedPeripheral('address', 'addrType', 'hci0')
+        self.assertEqual(tp._addr, 'address')
+        self.assertEqual(tp._addrType, 'addrType')
+        self.assertEqual(tp._iface_number, 0)
+        self.assertNotEqual(tp._dispatcher_thread, None)
+
+        # Schedule some methods to async queue and give them some time to resolve
+        tp.set_notify_handler(lambda: '')
+        tp.write(123, 'qwe')
+
+        tp._dispatcher_thread.join(1)
+        self.assertEqual(tp._dispatcher_thread.is_alive(), True)
+        tp.disconnect()
+
+        tp._dispatcher_thread.join(2)
+        self.assertEqual(tp._dispatcher_thread.is_alive(), False)

tests/test_comms.py

@@ -0,0 +1,40 @@
+import unittest
+
+from pylgbst.comms import *
+
+
+class ConnectionTestCase(unittest.TestCase):
+    def test_is_device_matched(self):
+        conn = Connection()
+
+        hub_address = '1a:2A:3A:4A:5A:6A'
+        other_address = 'A1:a2:a3:a4:a5:a6'
+        zero_address = '00:00:00:00:00:00'
+        hub_name = 'LEGO Move Hub'
+        other_name = 'HRM'
+
+        test_matrix = [
+            # address,      name,       hub_mac,        expected
+            (hub_address, hub_name, hub_address, None, True),
+            (hub_address, hub_name, None, hub_name, True),
+            (hub_address, None, hub_address, None, True),
+            (hub_address, None, None, hub_name, False),
+            (hub_address, other_name, hub_address, None, True),
+            (hub_address, other_name, None, hub_name, False),
+            (other_address, hub_name, hub_address, None, False),
+            (other_address, hub_name, None, hub_name, True),
+            (other_address, None, hub_address, None, False),
+            (other_address, None, None, hub_name, False),
+            (other_address, other_name, hub_address, None, False),
+            (other_address, other_name, None, hub_name, False),
+            (zero_address, hub_name, hub_address, None, False),
+            (zero_address, hub_name, None, hub_name, False),
+            (zero_address, None, hub_address, None, False),
+            (zero_address, None, None, hub_name, False),
+            (zero_address, other_name, hub_address, None, False),
+            (zero_address, other_name, None, hub_name, False),
+        ]
+
+        for address, name, hub_mac, fname, expected in test_matrix:
+            matched = conn._is_device_matched(address=address, dev_name=name, hub_mac=hub_mac, find_name=fname)
+            self.assertEqual(matched, expected)

tests/test_gatt.py

@@ -1,7 +1,7 @@
+import dbus
 import sys
 import unittest
 
-import dbus
 from gatt import DeviceManager
 
 from pylgbst.comms.cgatt import CustomDevice, GattConnection

tests/test_hub.py

@@ -0,0 +1,145 @@
+import time
+import unittest
+
+from pylgbst.hub import Hub, MoveHub
+from pylgbst.messages import MsgHubAction, MsgHubAlert, MsgHubProperties
+from pylgbst.peripherals import VisionSensor
+from pylgbst.utilities import usbyte
+from tests import ConnectionMock
+
+
+class HubTest(unittest.TestCase):
+    def test_hub_properties(self):
+        conn = ConnectionMock().connect()
+        hub = Hub(conn)
+
+        conn.notification_delayed('060001060600', 0.1)
+        msg = MsgHubProperties(MsgHubProperties.VOLTAGE_PERC, MsgHubProperties.UPD_REQUEST)
+        resp = hub.send(msg)
+        assert isinstance(resp, MsgHubProperties)
+        self.assertEqual(1, len(resp.parameters))
+        self.assertEqual(0, usbyte(resp.parameters, 0))
+
+        conn.notification_delayed('12000101064c45474f204d6f766520487562', 0.1)
+        msg = MsgHubProperties(MsgHubProperties.ADVERTISE_NAME, MsgHubProperties.UPD_REQUEST)
+        resp = hub.send(msg)
+        assert isinstance(resp, MsgHubProperties)
+        self.assertEqual(b"LEGO Move Hub", resp.parameters)
+
+        conn.wait_notifications_handled()
+
+    def test_device_attached(self):
+        conn = ConnectionMock().connect()
+        hub = Hub(conn)
+
+        # regular startup attaches
+        conn.notifications.append('0f0004010126000000001000000010')
+        conn.notifications.append('0f0004020125000000001000000010')
+
+        # detach and reattach
+        conn.notifications.append('0500040100')
+        conn.notifications.append('0500040200')
+        conn.notifications.append('0f0004010126000000001000000010')
+        conn.notifications.append('0f0004020125000000001000000010')
+
+        del hub
+        conn.wait_notifications_handled()
+
+    def test_hub_actions(self):
+        conn = ConnectionMock().connect()
+        hub = Hub(conn)
+        conn.notification_delayed('04000230', 0.1)
+        hub.send(MsgHubAction(MsgHubAction.UPSTREAM_SHUTDOWN))
+
+        conn.notification_delayed('04000231', 0.1)
+        hub.send(MsgHubAction(MsgHubAction.UPSTREAM_DISCONNECT))
+
+        conn.notification_delayed('04000232', 0.1)
+        hub.send(MsgHubAction(MsgHubAction.UPSTREAM_BOOT_MODE))
+        time.sleep(0.1)
+        conn.wait_notifications_handled()
+
+    def test_hub_alerts(self):
+        conn = ConnectionMock().connect()
+        hub = Hub(conn)
+        conn.notification_delayed('0600 03 0104ff', 0.1)
+        hub.send(MsgHubAlert(MsgHubAlert.LOW_VOLTAGE, MsgHubAlert.UPD_REQUEST))
+        conn.notification_delayed('0600 03 030400', 0.1)
+        hub.send(MsgHubAlert(MsgHubAlert.LOW_SIGNAL, MsgHubAlert.UPD_REQUEST))
+        conn.wait_notifications_handled()
+
+    def test_error(self):
+        conn = ConnectionMock().connect()
+        hub = Hub(conn)
+        conn.notification_delayed("0500056105", 0.1)
+        time.sleep(0.2)
+        conn.wait_notifications_handled()
+
+    def test_disconnect_off(self):
+        conn = ConnectionMock().connect()
+        hub = Hub(conn)
+        conn.notification_delayed("04000231", 0.1)
+        hub.disconnect()
+        self.assertEqual(b"04000202", conn.writes[1][1])
+
+        conn = ConnectionMock().connect()
+        hub = Hub(conn)
+        conn.notification_delayed("04000230", 0.1)
+        hub.switch_off()
+        self.assertEqual(b"04000201", conn.writes[1][1])
+
+    def test_sensor(self):
+        conn = ConnectionMock().connect()
+        conn.notifications.append("0f0004020125000000001000000010")  # add dev
+        hub = Hub(conn)
+        time.sleep(0.1)
+        dev = hub.peripherals[0x02]
+        assert isinstance(dev, VisionSensor)
+
+        conn.notification_delayed("0a004702080000000000", 0.1)
+        conn.notification_delayed("08004502ff0aff00", 0.2)  # value for sensor
+        self.assertEqual((255, 10.0), dev.get_sensor_data(VisionSensor.COLOR_DISTANCE_FLOAT))
+        self.assertEqual(b"0a004102080100000000", conn.writes.pop(1)[1])
+        self.assertEqual(b"0500210200", conn.writes.pop(1)[1])
+
+        vals = []
+        cb = lambda x, y=None: vals.append((x, y))
+
+        conn.notification_delayed("0a004702080100000001", 0.1)  # subscribe ack
+        dev.subscribe(cb, granularity=1)
+        self.assertEqual(b"0a004102080100000001", conn.writes.pop(1)[1])
+
+        conn.notification_delayed("08004502ff0aff00", 0.1)  # value for sensor
+        time.sleep(0.3)
+
+        conn.notification_delayed("0a004702080000000000", 0.3)  # unsubscribe ack
+        dev.unsubscribe(cb)
+        self.assertEqual(b"0a004102080100000000", conn.writes.pop(1)[1])
+
+        self.assertEqual([(255, 10.0)], vals)
+
+
+class MoveHubTest(unittest.TestCase):
+    def test_capabilities(self):
+        conn = ConnectionMock()
+        conn.notifications.append('0f00 04 02 0125000000001000000010')
+        conn.notifications.append('0f00 04 03 0126000000001000000010')
+        conn.notifications.append('0f00 04 00 0127000100000001000000')
+        conn.notifications.append('0f00 04 01 0127000100000001000000')
+        conn.notifications.append('0900 04 10 0227003738')
+        conn.notifications.append('0f00 04 32 0117000100000001000000')
+        conn.notifications.append('0f00 04 3a 0128000000000100000001')
+        conn.notifications.append('0f00 04 3b 0115000200000002000000')
+        conn.notifications.append('0f00 04 3c 0114000200000002000000')
+
+        conn.notification_delayed('12000101064c45474f204d6f766520487562', 1.1)
+        conn.notification_delayed('0b00010d06001653a0d1d4', 1.3)
+        conn.notification_delayed('060001060600', 1.5)
+        conn.notification_delayed('0600030104ff', 1.7)
+        MoveHub(conn.connect())
+        time.sleep(1)
+        conn.wait_notifications_handled()
+        self.assertEqual(b"0500010105", conn.writes[1][1])
+        self.assertEqual(b"0500010d05", conn.writes[2][1])
+        self.assertEqual(b"0500010605", conn.writes[3][1])
+        self.assertEqual(b"0500030103", conn.writes[4][1])

tests/test_movehub.py

@@ -1,141 +0,0 @@
-import time
-import unittest
-
-from pylgbst.movehub import MoveHub, MOVE_HUB_HARDWARE_HANDLE, PORT_LED, COLOR_RED
-from pylgbst.peripherals import LED, TiltSensor, COLORS
-from tests import log, HubMock, ConnectionMock, Thread
-
-HANDLE = MOVE_HUB_HARDWARE_HANDLE
-
-
-class GeneralTest(unittest.TestCase):
-    def _wait_notifications_handled(self, hub):
-        hub.connection.running = False
-        for _ in range(1, 180):
-            time.sleep(1)
-            log.debug("Waiting for notifications to process...")
-            if hub.connection.finished:
-                log.debug("Done waiting for notifications to process")
-                break
-
-    def test_led(self):
-        hub = HubMock()
-        led = LED(hub, PORT_LED)
-        led.set_color(COLOR_RED)
-        self.assertEqual("0801813201510009", hub.writes[0][1])
-
-    def test_tilt_sensor(self):
-        hub = HubMock()
-        hub.notify_mock.append((HANDLE, '0f00 04 3a 0128000000000100000001'))
-        time.sleep(1)
-
-        def callback(param1, param2=None, param3=None):
-            if param2 is None:
-                log.debug("Tilt: %s", TiltSensor.DUO_STATES[param1])
-            else:
-                log.debug("Tilt: %s %s %s", param1, param2, param3)
-
-        self._inject_notification(hub, '0a00 47 3a 090100000001', 1)
-        hub.tilt_sensor.subscribe(callback)
-        hub.notify_mock.append((HANDLE, "0500453a05"))
-        hub.notify_mock.append((HANDLE, "0a00473a010100000001"))
-        time.sleep(1)
-        self._inject_notification(hub, '0a00 47 3a 090100000001', 1)
-        hub.tilt_sensor.subscribe(callback, TiltSensor.MODE_2AXIS_SIMPLE)
-
-        hub.notify_mock.append((HANDLE, "0500453a09"))
-        time.sleep(1)
-
-        self._inject_notification(hub, '0a00 47 3a 090100000001', 1)
-        hub.tilt_sensor.subscribe(callback, TiltSensor.MODE_2AXIS_FULL)
-        hub.notify_mock.append((HANDLE, "0600453a04fe"))
-        time.sleep(1)
-
-        self._inject_notification(hub, '0a00 47 3a 090100000001', 1)
-        hub.tilt_sensor.unsubscribe(callback)
-        self._wait_notifications_handled(hub)
-        # TODO: assert
-
-    def test_motor(self):
-        conn = ConnectionMock()
-        conn.notifications.append((14, '0900 04 39 0227003738'))
-        hub = HubMock(conn)
-        time.sleep(0.1)
-
-        conn.notifications.append((14, '050082390a'))
-        hub.motor_AB.timed(1.5)
-        self.assertEqual("0d018139110adc056464647f03", conn.writes[0][1])
-
-        conn.notifications.append((14, '050082390a'))
-        hub.motor_AB.angled(90)
-        self.assertEqual("0f018139110c5a0000006464647f03", conn.writes[1][1])
-
-    def test_capabilities(self):
-        conn = ConnectionMock()
-        conn.notifications.append((14, '0f00 04 01 0125000000001000000010'))
-        conn.notifications.append((14, '0f00 04 02 0126000000001000000010'))
-        conn.notifications.append((14, '0f00 04 37 0127000100000001000000'))
-        conn.notifications.append((14, '0f00 04 38 0127000100000001000000'))
-        conn.notifications.append((14, '0900 04 39 0227003738'))
-        conn.notifications.append((14, '0f00 04 32 0117000100000001000000'))
-        conn.notifications.append((14, '0f00 04 3a 0128000000000100000001'))
-        conn.notifications.append((14, '0f00 04 3b 0115000200000002000000'))
-        conn.notifications.append((14, '0f00 04 3c 0114000200000002000000'))
-        conn.notifications.append((14, '0f00 8202 01'))
-        conn.notifications.append((14, '0f00 8202 0a'))
-
-        self._inject_notification(conn, '1200 0101 06 4c45474f204d6f766520487562', 1)
-        self._inject_notification(conn, '1200 0108 06 4c45474f204d6f766520487562', 2)
-        self._inject_notification(conn, '0900 47 3c 0227003738', 3)
-        self._inject_notification(conn, '0600 45 3c 020d', 4)
-        self._inject_notification(conn, '0900 47 3c 0227003738', 5)
-        hub = MoveHub(conn)
-        # demo_all(hub)
-        self._wait_notifications_handled(hub)
-
-    def test_color_sensor(self):
-        #
-        hub = HubMock()
-        hub.notify_mock.append((HANDLE, '0f00 04010125000000001000000010'))
-        time.sleep(1)
-
-        def callback(color, unk1, unk2=None):
-            name = COLORS[color] if color is not None else 'NONE'
-            log.info("Color: %s %s %s", name, unk1, unk2)
-
-        self._inject_notification(hub, '0a00 4701090100000001', 1)
-        hub.color_distance_sensor.subscribe(callback)
-
-        hub.notify_mock.append((HANDLE, "08004501ff0aff00"))
-        time.sleep(1)
-        # TODO: assert
-        self._inject_notification(hub, '0a00 4701090100000001', 1)
-        hub.color_distance_sensor.unsubscribe(callback)
-        self._wait_notifications_handled(hub)
-
-    def test_button(self):
-        hub = HubMock()
-        time.sleep(1)
-
-        def callback(pressed):
-            log.info("Pressed: %s", pressed)
-
-        hub.notify_mock.append((HANDLE, "060001020600"))
-        hub.button.subscribe(callback)
-
-        hub.notify_mock.append((HANDLE, "060001020601"))
-        hub.notify_mock.append((HANDLE, "060001020600"))
-        time.sleep(1)
-        # TODO: assert
-        hub.button.unsubscribe(callback)
-        self._wait_notifications_handled(hub)
-
-    def _inject_notification(self, hub, notify, pause):
-        def inject():
-            time.sleep(pause)
-            if isinstance(hub, ConnectionMock):
-                hub.notifications.append((HANDLE, notify))
-            else:
-                hub.notify_mock.append((HANDLE, notify))
-
-        Thread(target=inject).start()

tests/test_peripherals.py

@@ -0,0 +1,242 @@
+import logging
+import time
+import unittest
+
+from pylgbst.hub import MoveHub
+from pylgbst.peripherals import LEDRGB, TiltSensor, COLOR_RED, Button, Current, Voltage, VisionSensor, \
+    EncodedMotor
+from tests import HubMock
+
+
+class PeripheralsTest(unittest.TestCase):
+
+    def test_button(self):
+        hub = HubMock()
+        time.sleep(0.1)
+        button = Button(hub)
+        hub.peripherals[0x00] = button
+
+        vals = []
+
+        def callback(pressed):
+            vals.append(pressed)
+
+        hub.connection.notification_delayed("060001020600", 0.0)
+        button.subscribe(callback)
+        time.sleep(0.1)
+
+        hub.connection.notification_delayed("060001020600", 0.1)
+        hub.connection.notification_delayed("060001020601", 0.2)
+        hub.connection.notification_delayed("060001020600", 0.3)
+        time.sleep(0.4)
+
+        button.unsubscribe(callback)
+        time.sleep(0.1)
+        hub.connection.wait_notifications_handled()
+
+        self.assertEqual([0, 1, 0], vals)
+        self.assertEqual(b"0500010202", hub.writes[1][1])
+        self.assertEqual(b"0500010203", hub.writes[2][1])
+
+    def test_led(self):
+        hub = HubMock()
+        hub.led = LEDRGB(hub, MoveHub.PORT_LED)
+        hub.peripherals[MoveHub.PORT_LED] = hub.led
+
+        hub.connection.notification_delayed("0a004732000100000000", 0.1)
+        hub.connection.notification_delayed("050082320a", 0.2)
+        hub.led.set_color(COLOR_RED)
+        self.assertEqual(b"0a004132000100000000", hub.writes.pop(1)[1])
+        self.assertEqual(b"0800813211510009", hub.writes.pop(1)[1])
+
+        hub.connection.notification_delayed("0a004732010100000000", 0.1)
+        hub.connection.notification_delayed("050082320a", 0.2)
+        hub.led.set_color((32, 64, 96))
+        self.assertEqual(b"0a004132010100000000", hub.writes.pop(1)[1])
+        self.assertEqual(b"0a008132115101204060", hub.writes.pop(1)[1])
+
+    def test_current(self):
+        hub = HubMock()
+        time.sleep(0.1)
+        current = Current(hub, MoveHub.PORT_CURRENT)
+        hub.peripherals[MoveHub.PORT_CURRENT] = current
+
+        vals = []
+
+        def callback(val):
+            vals.append(val)
+
+        hub.connection.notification_delayed("0a00473b000100000001", 0.1)
+        current.subscribe(callback)
+
+        hub.connection.notification_delayed("0600453ba400", 0.1)
+        time.sleep(0.2)
+
+        hub.connection.notification_delayed("0a00473b000000000000", 0.1)
+        current.unsubscribe(callback)
+        hub.connection.wait_notifications_handled()
+
+        self.assertEqual([97.87936507936507], vals)
+        self.assertEqual(b"0a00413b000100000001", hub.writes[1][1])
+        self.assertEqual(b"0a00413b000100000000", hub.writes[2][1])
+
+    def test_voltage(self):
+        hub = HubMock()
+        time.sleep(0.1)
+        voltage = Voltage(hub, MoveHub.PORT_VOLTAGE)
+        hub.peripherals[MoveHub.PORT_VOLTAGE] = voltage
+
+        vals = []
+
+        def callback(val):
+            vals.append(val)
+
+        hub.connection.notification_delayed("0a00473c000100000001", 0.1)
+        voltage.subscribe(callback)
+
+        hub.connection.notification_delayed("0600453c9907", 0.1)
+        time.sleep(0.2)
+
+        hub.connection.notification_delayed("0a00473c000000000000", 0.1)
+        voltage.unsubscribe(callback)
+        hub.connection.wait_notifications_handled()
+
+        self.assertEqual([4.79630105317236], vals)
+        self.assertEqual(b"0a00413c000100000001", hub.writes[1][1])
+        self.assertEqual(b"0a00413c000100000000", hub.writes[2][1])
+
+    def test_tilt_sensor(self):
+        hub = HubMock()
+        sensor = TiltSensor(hub, MoveHub.PORT_TILT_SENSOR)
+        hub.peripherals[MoveHub.PORT_TILT_SENSOR] = sensor
+
+        hub.connection.notification_delayed('0a00473a000100000000', 0.05)
+        hub.connection.notification_delayed('0600453a0201', 0.1)
+        self.assertEqual((2, 1), sensor.get_sensor_data(TiltSensor.MODE_2AXIS_ANGLE))
+
+        hub.connection.notification_delayed('0a00473a010100000000', 0.05)
+        hub.connection.notification_delayed('0500453a00', 0.1)
+        self.assertEqual((0,), sensor.get_sensor_data(TiltSensor.MODE_2AXIS_SIMPLE))
+
+        hub.connection.notification_delayed('0a00473a020100000000', 0.05)
+        hub.connection.notification_delayed('0600453a0201', 0.1)
+        self.assertEqual((2,), sensor.get_sensor_data(TiltSensor.MODE_3AXIS_SIMPLE))
+
+        hub.connection.notification_delayed('0a00473a030100000000', 0.05)
+        hub.connection.notification_delayed('0800453a00000000', 0.1)
+        self.assertEqual((0,), sensor.get_sensor_data(TiltSensor.MODE_IMPACT_COUNT))
+
+        hub.connection.notification_delayed('0a00473a040100000000', 0.05)
+        hub.connection.notification_delayed('0700453afd0140', 0.1)
+        self.assertEqual((-3, 1, 64), sensor.get_sensor_data(TiltSensor.MODE_3AXIS_ACCEL))
+
+        hub.connection.notification_delayed('0a00473a050100000000', 0.05)
+        hub.connection.notification_delayed('0500453a00', 0.1)
+        self.assertEqual((0,), sensor.get_sensor_data(TiltSensor.MODE_ORIENT_CF))
+
+        hub.connection.notification_delayed('0a00473a060100000000', 0.05)
+        hub.connection.notification_delayed('0600453a7f14', 0.1)
+        self.assertEqual((127,), sensor.get_sensor_data(TiltSensor.MODE_IMPACT_CF))
+
+        hub.connection.notification_delayed('0a00473a070100000000', 0.05)
+        hub.connection.notification_delayed('0700453a00feff', 0.1)
+        self.assertEqual((0, 254, 255), sensor.get_sensor_data(TiltSensor.MODE_CALIBRATION))
+
+        hub.connection.wait_notifications_handled()
+
+    def test_motor(self):
+        hub = HubMock()
+        motor = EncodedMotor(hub, MoveHub.PORT_D)
+        hub.peripherals[MoveHub.PORT_D] = motor
+
+        hub.connection.notification_delayed('050082030a', 0.1)
+        motor.start_power(1.0)
+        self.assertEqual(b"07008103110164", hub.writes.pop(1)[1])
+
+        hub.connection.notification_delayed('050082030a', 0.1)
+        motor.stop()
+        self.assertEqual(b"0c0081031109000064647f03", hub.writes.pop(1)[1])
+
+        hub.connection.notification_delayed('050082030a', 0.1)
+        motor.set_acc_profile(1.0)
+        self.assertEqual(b"090081031105e80300", hub.writes.pop(1)[1])
+
+        hub.connection.notification_delayed('050082030a', 0.1)
+        motor.set_dec_profile(1.0)
+        self.assertEqual(b"090081031106e80300", hub.writes.pop(1)[1])
+
+        hub.connection.notification_delayed('050082030a', 0.1)
+        motor.start_speed(1.0)
+        self.assertEqual(b"090081031107646403", hub.writes.pop(1)[1])
+
+        hub.connection.notification_delayed('050082030a', 0.1)
+        motor.stop()
+        self.assertEqual(b"0c0081031109000064647f03", hub.writes.pop(1)[1])
+
+        logging.debug("\n\n")
+        hub.connection.notification_delayed('0500820301', 0.1)
+        hub.connection.notification_delayed('050082030a', 0.2)
+        motor.timed(1.0)
+        self.assertEqual(b"0c0081031109e80364647f03", hub.writes.pop(1)[1])
+
+        hub.connection.notification_delayed('0500820301', 0.1)
+        hub.connection.notification_delayed('050082030a', 0.2)
+        motor.angled(180)
+        self.assertEqual(b"0e008103110bb400000064647f03", hub.writes.pop(1)[1])
+
+        hub.connection.notification_delayed('050082030a', 0.2)
+        motor.preset_encoder(-180)
+        self.assertEqual(b"0b0081031151024cffffff", hub.writes.pop(1)[1])
+
+        hub.connection.notification_delayed('0500820301', 0.1)
+        hub.connection.notification_delayed('050082030a', 0.2)
+        motor.goto_position(0)
+        self.assertEqual(b"0e008103110d0000000064647f03", hub.writes.pop(1)[1])
+
+        hub.connection.wait_notifications_handled()
+
+    def test_motor_sensor(self):
+        hub = HubMock()
+        motor = EncodedMotor(hub, MoveHub.PORT_C)
+        hub.peripherals[MoveHub.PORT_C] = motor
+
+        vals = []
+
+        def callback(*args):
+            vals.append(args)
+
+        hub.connection.notification_delayed('0a004702020100000001', 0.1)
+        motor.subscribe(callback)
+
+        hub.connection.notification_delayed("0800450200000000", 0.1)
+        hub.connection.notification_delayed("08004502ffffffff", 0.2)
+        hub.connection.notification_delayed("08004502feffffff", 0.3)
+        time.sleep(0.4)
+
+        hub.connection.notification_delayed('0a004702020000000000', 0.1)
+        motor.unsubscribe(callback)
+        hub.connection.wait_notifications_handled()
+
+        self.assertEqual([(0,), (-1,), (-2,)], vals)
+
+    def test_color_sensor(self):
+        hub = HubMock()
+        cds = VisionSensor(hub, MoveHub.PORT_C)
+        hub.peripherals[MoveHub.PORT_C] = cds
+
+        vals = []
+
+        def callback(*args):
+            vals.append(args)
+
+        hub.connection.notification_delayed('0a00 4702080100000001', 0.1)
+        cds.subscribe(callback)
+
+        hub.connection.notification_delayed("08004502ff0aff00", 0.1)
+        time.sleep(0.2)
+
+        hub.connection.notification_delayed('0a00 4702090100000001', 0.1)
+        cds.unsubscribe(callback)
+        hub.connection.wait_notifications_handled()
+
+        self.assertEqual([(255, 10.0)], vals)

tests/test_pygatt.py

@@ -5,6 +5,7 @@ import serial
 from pygatt import BLEAddressType
 from pygatt.backends.bgapi.bgapi import MAX_CONNECTION_ATTEMPTS
 from pygatt.backends.bgapi.device import BGAPIBLEDevice
+from pygatt.backends.bgapi.util import USBSerialDeviceInfo
 
 from pylgbst.comms.cpygatt import GattoolConnection
 from tests import log
@@ -55,6 +56,9 @@ class BlueGigaBackendMock(pygatt.BGAPIBackend):
         device = BGAPIBLEDeviceMock("address", 0, self)
         return device
 
+    def _detect_device_port(self):
+        return USBSerialDeviceInfo().port_name
+
 
 class BlueGigaTests(unittest.TestCase):
     def test_1(self):