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

@@ -1,3 +1,7 @@
 .idea
 *.iml
-*.pyc
+*.pyc
+build
+*.avi
+test_real.py
+.vscode/settings.json

.travis.yml

@@ -1,23 +1,37 @@
 sudo: false
 language: python
-
-matrix:
-  include:
-  - os: linux
-    python: 2.7
-  - os: linux
-    python: 3.5
+python:
+  - 3.6
+  - 3.8
 
 addons:
   apt:
     packages:
-    - libboost-python-dev
-    - libboost-thread-dev
-    - libbluetooth-dev
-    - libglib2.0-dev
+      - libboost-python-dev
+      - libboost-thread-dev
+      - libbluetooth-dev
+
+      - libglib2.0-dev
+      - libdbus-1-dev
+      - libdbus-glib-1-dev
+      - libgirepository-1.0-1
+      - libgirepository1.0-dev
+
+      - bluez
 install:
-- pip install codecov gattlib
-script: coverage run --source=. `which nosetests` .
+  - 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 --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,77 +1,122 @@
-# Python library to interact with LEGO Move Hub
+# Python library to interact with Move Hub / PoweredUp Hubs
 
-Best way to start is to look into [`demo.py`](demo.py) file, and run it.
+_Move Hub is central controller block of [LEGO® Boost Robotics Set](https://www.lego.com/themes/boost)._
+
+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.
+
+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
+
+[![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)
 
-If you have Vernie assembled, you might look into and run scripts from [`vernie`](vernie/) directory.
 
 ## Features
 
-- auto-detect and connect for Bluetooth device
-- auto-detects devices connected to Hub
-- angled and timed movement for motors
-- LED color change
-- motors: angled and timed movement, rotation sensor subscription
-- push button status subscription
-- tilt sensor subscription: 2 axis, 3 axis, bump detect modes
-- color & distance sensor: several modes to measure distance, color and luminosity
-- 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.2.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 import MoveHub
+from pylgbst.hub import MoveHub
 
 hub = MoveHub()
 
-for device in hub.devices:
+for device in hub.peripherals:
     print(device)
 ```
 
-TODO: more usage instructions
+Each peripheral kind has own methods to do actions and/or get sensor data. See [features](#features) list for individual doc pages.
 
-### General Information
-hub's devices detect process & fields to access them
-general subscription modes & granularity info
+## Bluetooth Backend Prerequisites
 
-### Motors
-### Motor Rotation Sensors
-### Tilt Sensor
-### Color & Distance Sensor
-### LED
-### Push Button
-### Power Voltage & Battery
+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.
+
+_Please let author know if you have discovered any compatibility/preprequisite details, so we will update this section to help future users_
+
+Depending on backend type, you might need Linux `sudo` to be used when running Python.
+
+### Bluetooth Connection Options
+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 `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 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 pass that to class or function of getting a connection. Then pass connection object to `MoveHub` constructor. Like this:
+```python
+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)
+```
 
 
 ## 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. 
 
-```
-sudo python -c "from pylgbst.comms import *; import logging; logging.basicConfig(level=logging.DEBUG); DebugServer(BLEConnection().connect()).start()"
+There is `DebugServerConnection` class that you can use with it, instead of `BLEConnection`. 
+
+Starting debug server is done like this (you may need to run it with `sudo`, depending on your BLE backend):
+```bash
+python -c "import logging; logging.basicConfig(level=logging.DEBUG); \
+                import pylgbst; pylgbst.start_debug_server()"
 ```
 
-## Roadmap
+Then push green button on MoveHub, so permanent BLE connection will be established.
 
-- handle device detach and device attach events on ports C/D
-- experiment with motor commands, find what is hidden there
-- Give nice documentation examples, don't forget to mention logging
+## 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
-- generalize getting device info + give constants (low priority)
+- make debug server to re-establish BLE connection on loss
 
 ## Links
 
-- https://github.com/JorgePe/BOOSTreveng - source of protocol knowledge
-- 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/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
+- 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

@@ -1,7 +1,10 @@
 # coding=utf-8
+import time
 from time import sleep
 
 from pylgbst import *
+from pylgbst.hub import MoveHub
+from pylgbst.peripherals import EncodedMotor, TiltSensor, Current, Voltage, COLORS, COLOR_BLACK
 
 log = logging.getLogger("demo")
 
@@ -9,7 +12,13 @@ log = logging.getLogger("demo")
 def demo_led_colors(movehub):
     # LED colors demo
     log.info("LED colors demo")
-    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)
@@ -17,7 +26,7 @@ def demo_led_colors(movehub):
 
 def demo_motors_timed(movehub):
     log.info("Motors movement demo: timed")
-    for level in range(0, 101, 5):
+    for level in range(0, 101, 10):
         level /= 100.0
         log.info("Speed level: %s%%", level * 100)
         movehub.motor_A.timed(0.2, level)
@@ -70,11 +79,11 @@ def demo_tilt_sensor_simple(movehub):
     demo_tilt_sensor_simple.cnt = 0
     limit = 10
 
-    def callback(param1):
+    def callback(state):
         demo_tilt_sensor_simple.cnt += 1
-        log.info("Tilt #%s of %s: %s", demo_tilt_sensor_simple.cnt, limit, TILT_STATES[param1])
+        log.info("Tilt #%s of %s: %s=%s", demo_tilt_sensor_simple.cnt, limit, TiltSensor.TRI_STATES[state], state)
 
-    movehub.tilt_sensor.subscribe(callback)
+    movehub.tilt_sensor.subscribe(callback, mode=TiltSensor.MODE_3AXIS_SIMPLE)
     while demo_tilt_sensor_simple.cnt < limit:
         time.sleep(1)
 
@@ -90,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=TILT_MODE_FULL)
+    movehub.tilt_sensor.subscribe(callback, mode=TiltSensor.MODE_3AXIS_ACCEL)
     while demo_tilt_sensor_simple.cnt < limit:
         time.sleep(1)
 
@@ -106,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
@@ -136,36 +145,120 @@ 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():  # demo_motor_sensors.states < limit:
+    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)
 
 
+def demo_voltage(movehub):
+    def callback1(value):
+        log.info("Amperage: %s", value)
+
+    def callback2(value):
+        log.info("Voltage: %s", value)
+
+    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.VOLTAGE_L, granularity=0)
+    movehub.voltage.subscribe(callback2, mode=Voltage.VOLTAGE_L, granularity=1)
+    time.sleep(5)
+    movehub.current.unsubscribe(callback1)
+    movehub.voltage.unsubscribe(callback2)
+
+
 def demo_all(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.DEBUG)
-
+    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.warning("Failed to use debug server: %s", traceback.format_exc())
-        connection = BLEConnection().connect()
+        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(connection)
-    demo_all(hub)
+    hub = MoveHub(**parameters)
+    try:
+        demo = DEMO_CHOICES[options.demo]
+        demo(hub)
+    finally:
+        hub.disconnect()

examples/harmonograph/__init__.py

@@ -0,0 +1,18 @@
+import logging
+import traceback
+
+from pylgbst import get_connection_auto
+from pylgbst.comms import DebugServerConnection
+from pylgbst.hub import MoveHub
+
+if __name__ == '__main__':
+    logging.basicConfig(level=logging.INFO)
+    hub = MoveHub()
+    try:
+        hub.motor_AB.start_power(0.45, 0.45)
+        hub.motor_external.angled(12590, 0.1)
+        # time.sleep(180)
+    finally:
+        hub.motor_AB.stop()
+        if hub.motor_external:
+            hub.motor_external.stop()

examples/plotter/__init__.py

@@ -0,0 +1,236 @@
+import logging
+import math
+import time
+
+from pylgbst.peripherals import VisionSensor, COLOR_RED, COLOR_CYAN, COLORS
+
+
+class Plotter(object):
+    MOTOR_RATIO = 1.425
+    ROTATE_UNIT = 2100
+
+    def __init__(self, hub, base_speed=1.0):
+        """
+
+        :type hub: pylgbst.hub.MoveHub
+        """
+        self._hub = hub
+        self.caret = self._hub.motor_A
+        self.wheels = self._hub.motor_B
+        self.both = self._hub.motor_AB
+
+        self.base_speed = float(base_speed)
+        self.xpos = 0
+        self.ypos = 0
+        self.is_tool_down = False
+        self._marker_color = False
+        self.__last_wheel_dir = 1
+
+    def initialize(self):
+        self._reset_caret()
+        self._compensate_wheels_backlash(1)
+        self._compensate_wheels_backlash(-1)
+        self.xpos = 0
+        self.ypos = 0
+        self.is_tool_down = False
+
+    def _reset_caret(self):
+        if not self._hub.vision_sensor:
+            logging.warning("No color/distance sensor, cannot center caret")
+            return
+
+        self._hub.vision_sensor.subscribe(self._on_distance, mode=VisionSensor.COLOR_DISTANCE_FLOAT)
+        self.caret.timed(0.5, 1)
+        try:
+            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.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.vision_sensor.unsubscribe(self._on_distance)
+
+        if self._marker_color == COLOR_CYAN:
+            self.move(-0.1, 0)
+        else:
+            self.move(-1.0, 0)
+
+    def _on_distance(self, color, distance):
+        self._marker_color = None
+        logging.debug("Color: %s, distance %s", COLORS[color], distance)
+        if color in (COLOR_RED, COLOR_CYAN):
+            if distance <= 3:
+                self._marker_color = color
+
+    def _compensate_wheels_backlash(self, movy):
+        """
+        corrects backlash of wheels gear system
+        """
+        if not movy:
+            return
+        wheel_dir = movy / abs(movy)
+        if wheel_dir == -self.__last_wheel_dir:
+            self.wheels.angled(270, -wheel_dir)
+        self.__last_wheel_dir = wheel_dir
+
+    def finalize(self):
+        if self.is_tool_down:
+            self._tool_up()
+        self.both.stop()
+
+    def _tool_down(self):
+        self.is_tool_down = True
+        self._hub.motor_external.angled(-270, 1)
+        time.sleep(1.0)  # for laser to heat up
+
+    def _tool_up(self):
+        self._hub.motor_external.angled(270, 1)
+        self.is_tool_down = False
+
+    def move(self, movx, movy):
+        if self.is_tool_down:
+            self._tool_up()
+        self._transfer_to(movx, movy)
+
+    def line(self, movx, movy):
+        if not self.is_tool_down:
+            self._tool_down()
+        self._transfer_to(movx, movy)
+
+    def _transfer_to(self, movx, movy):
+        if not movy and not movx:
+            logging.warning("No movement, ignored")
+            return
+
+        self._compensate_wheels_backlash(movy)
+
+        self.xpos += movx
+        self.ypos += movy
+
+        length, speed_a, speed_b = self._calc_motor_angled(movx, movy * self.MOTOR_RATIO)
+        logging.info("Motors: %.3f with %.3f/%.3f", length, speed_a, speed_b)
+
+        if not speed_b:
+            self.caret.angled(length * 2.0, -speed_a * self.base_speed * 0.75)  # slow down to cut better
+        elif not speed_a:
+            self.wheels.angled(length * 2.0, -speed_b * self.base_speed * 0.75)  # slow down to cut better
+        else:
+            self.both.angled(length, -speed_a * self.base_speed, -speed_b * self.base_speed)
+
+    @staticmethod
+    def _calc_motor_timed(movx, movy):
+        amovx = float(abs(movx))
+        amovy = float(abs(movy))
+
+        length = max(amovx, amovy)
+
+        speed_a = (movx / float(amovx)) if amovx else 0.0
+        speed_b = (movy / float(amovy)) if amovy else 0.0
+
+        if amovx >= amovy:
+            speed_b = movy / amovx
+        else:
+            speed_a = movx / amovy
+
+        assert -1 <= speed_a <= 1
+        assert -1 <= speed_b <= 1
+
+        return length, speed_a, speed_b
+
+    @staticmethod
+    def _calc_motor_angled(movx, movy):
+        amovx = abs(movx)
+        amovy = abs(movy)
+        if amovx >= amovy:
+            ax = amovy / (amovx + amovy)
+            spd_b = ax
+            if spd_b < 0.05:
+                spd_b = 0
+            spd_a = (1.0 - spd_b)
+            rotate = Plotter.ROTATE_UNIT * amovx * (1.0 + spd_b / spd_a)
+            logging.info("A: movx=%s, movy=%s, ax=%s", movx, movy, ax)
+        else:
+            ax = amovx / (amovx + amovy)
+            spd_a = ax
+            if spd_a < 0.05:
+                spd_a = 0
+            spd_b = (1.0 - spd_a)
+            rotate = Plotter.ROTATE_UNIT * amovy * (1.0 + spd_a / spd_b)
+            logging.info("B: movx=%s, movy=%s, ax=%s", movx, movy, ax)
+
+        assert 0 <= spd_a <= 1
+        assert 0 <= spd_b <= 1
+
+        spd_a *= (movx / amovx) if amovx else 0
+        spd_b *= (movy / amovy) if amovy else 0
+        return rotate, spd_a, spd_b
+
+    def circle(self, radius):
+        if not self.is_tool_down:
+            self._tool_down()
+
+        parts = int(2 * math.pi * radius * 7)
+        dur = 0.025
+        logging.info("Circle of radius %s, %s parts with %s time", radius, parts, dur)
+        speeds = []
+        for x in range(0, parts):
+            speed_a = math.sin(x * 2.0 * math.pi / float(parts))
+            speed_b = math.cos(x * 2.0 * math.pi / float(parts))
+            speeds.append((speed_a, speed_b))
+            logging.debug("A: %s, B: %s", speed_a, speed_b)
+        speeds.append((0, 0))
+
+        for speed_a, speed_b in speeds:
+            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.start_power(spa, spb)
+            time.sleep(dur)
+
+    def spiral(self, rounds, growth):
+        if not self.is_tool_down:
+            self._tool_down()
+
+        dur = 0.00
+        parts = 16
+        speeds = []
+        for r in range(0, rounds):
+            logging.info("Round: %s", r)
+
+            for x in range(0, parts):
+                speed_a = math.sin(x * 2.0 * math.pi / float(parts))
+                speed_b = math.cos(x * 2.0 * math.pi / float(parts))
+                dur += growth
+                speeds.append((speed_a, speed_b, dur))
+                logging.debug("A: %s, B: %s", speed_a, speed_b)
+        speeds.append((0, 0, 0))
+
+        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.start_power(spa, spb)
+            logging.info("Motor speeds: %.3f / %.3f", spa, spb)
+            time.sleep(dur)
+
+    def rectangle(self, width, height, solid=False):
+        self.line(width, 0)
+        self.line(0, height)
+        self.line(-width, 0)
+        self.line(0, -height)
+
+        if solid:
+            max_step = 0.01
+            rounds = int(math.ceil(height / max_step))
+            step = height / rounds
+            flip = 1
+            for r in range(1, rounds):
+                self.line(0, step)
+                self.line(width * flip, 0)
+                flip = -flip

examples/plotter/lego.py

@@ -0,0 +1,81 @@
+def lego(plotter, t):
+    h = t * 5.0
+    w = t * 3.0
+
+    plotter.move(-t * 2.0, 0)
+    l(h, plotter, t, w)
+    plotter.move(0, w + t)
+    e(h, plotter, t, w)
+    plotter.move(0, w + t)
+    g(plotter, t)
+    plotter.move(0, w + t)
+    o(plotter, t)
+
+
+def o(plotter, t):
+    # O
+    plotter.move(t, 0)
+    plotter.line(3 * t, 0)
+    plotter.line(t, t)
+    plotter.line(0, t)
+    plotter.line(-t, t)
+    plotter.line(-3 * t, 0)
+    plotter.line(-t, -t)
+    plotter.line(0, -t)
+    plotter.line(t, -t)
+    plotter.move(0, t)
+    plotter.line(3 * t, 0)
+    plotter.line(0, t)
+    plotter.line(-3 * t, 0)
+    plotter.line(0, -t)
+
+
+def g(plotter, t):
+    # G
+    plotter.move(t, 0)
+    plotter.line(3 * t, 0)
+    plotter.line(t, t)
+    plotter.line(0, t)
+    plotter.line(-t, t)
+    plotter.line(-t, 0)
+    plotter.line(0, -t)
+    plotter.line(t, 0)
+    plotter.line(0, -t)
+    plotter.line(-3 * t, 0)
+    plotter.line(0, t)
+    plotter.line(t * 0.25, 0)
+    plotter.line(0, -t * 0.25)
+    plotter.line(t * 0.75, 0)
+    plotter.line(0, t * 1.25)
+    plotter.line(-3 * t, 0)
+    plotter.line(0, -t)
+    plotter.line(t, 0)
+    plotter.line(0, -t)
+    plotter.line(t, -t)
+    plotter.move(-t, 0)
+
+
+def e(h, plotter, t, w):
+    # E
+    plotter.line(h, 0)
+    plotter.line(0, w)
+    plotter.line(-t, 0)
+    plotter.line(0, -2 * t)
+    plotter.line(-t, 0)
+    plotter.line(0, t)
+    plotter.line(-t, 0)
+    plotter.line(0, -t)
+    plotter.line(-t, 0)
+    plotter.line(0, 2 * t)
+    plotter.line(-t, 0)
+    plotter.line(0, -w)
+
+
+def l(h, plotter, t, w):
+    # L
+    plotter.line(h, 0)
+    plotter.line(0, t)
+    plotter.line(-(h - t), 0)
+    plotter.line(0, 2 * t)
+    plotter.line(-t, 0)
+    plotter.line(0, -w)

examples/plotter/try.py

@@ -0,0 +1,255 @@
+# coding=utf-8
+import logging
+import time
+
+from examples.plotter import Plotter
+from pylgbst.hub import EncodedMotor, MoveHub
+from tests import HubMock
+
+
+def moves():
+    plotter.move(-FIELD_WIDTH, 0)
+    plotter.move(FIELD_WIDTH * 2, 0)
+    plotter.move(-FIELD_WIDTH, 0)
+
+    plotter.move(FIELD_WIDTH, FIELD_WIDTH)
+    plotter.move(-FIELD_WIDTH, -FIELD_WIDTH)
+
+    plotter.move(0, FIELD_WIDTH)
+    plotter.move(0, -FIELD_WIDTH)
+
+
+def cross():
+    plotter.line(FIELD_WIDTH, FIELD_WIDTH)
+    plotter.move(-FIELD_WIDTH, 0)
+    plotter.line(FIELD_WIDTH, -FIELD_WIDTH)
+
+
+def square():
+    plotter.line(FIELD_WIDTH, 0)
+    plotter.line(0, FIELD_WIDTH)
+    plotter.line(-FIELD_WIDTH, 0)
+    plotter.line(0, -FIELD_WIDTH)
+
+
+def triangle():
+    plotter.line(FIELD_WIDTH, 0)
+    plotter.line(0, FIELD_WIDTH)
+    plotter.line(-FIELD_WIDTH, -FIELD_WIDTH)
+
+
+def romb():
+    plotter.move(-FIELD_WIDTH, 0)
+    plotter.line(FIELD_WIDTH, FIELD_WIDTH * 2)
+    plotter.line(FIELD_WIDTH, -FIELD_WIDTH * 2)
+    plotter.line(-FIELD_WIDTH, -FIELD_WIDTH * 2)
+    plotter.line(-FIELD_WIDTH, FIELD_WIDTH * 2)
+
+
+def circles():
+    plotter.move(FIELD_WIDTH / 4.0, 0)
+    plotter.circle(FIELD_WIDTH / 2.0)
+
+    plotter.move(FIELD_WIDTH / 2.0, 0)
+    plotter.circle(FIELD_WIDTH)
+
+
+def square_spiral():
+    rounds = 5
+    step = plotter.base_speed / rounds / 5.0
+    for r in range(1, rounds):
+        plotter.line(step * r * 4, 0)
+        plotter.line(0, step * (r * 4 + 1))
+        plotter.line(-step * (r * 4 + 2), 0)
+        plotter.line(0, -step * (r * 4 + 3))
+    plotter.line(step * 2.0, step * 2.0)  # cut
+
+
+def christmas_tree():
+    t = FIELD_WIDTH / 3
+    plotter.line(t, t)
+    plotter.line(-t * 0.5, 0)
+    plotter.line(t, t)
+    plotter.line(-t * 0.5, 0)
+    plotter.line(t, t)
+
+    plotter.line(-t * 3.5, 0)
+
+    plotter.line(t, -t)
+    plotter.line(-t * 0.5, 0)
+    plotter.line(t, -t)
+    plotter.line(-t * 0.5, 0)
+    plotter.line(t, -t)
+
+
+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.start_power(s, -s)
+        time.sleep(1)
+    for s in reversed(speeds):
+        logging.info("%s", s)
+        plotter.both.start_power(-s, s)
+        time.sleep(1)
+
+
+def snowflake(scale=1.0):
+    a = [300, 232,
+         351, 144,
+         307, 68,
+         350, 45,
+         379, 94,
+         413, 36,
+         456, 61,
+         422, 119,
+         482, 118,
+         481, 167,
+         394, 168,
+         343, 256,
+         444, 256,
+         488, 179,
+         530, 204,
+         500, 256,
+         569, 256,
+         582, 280]
+    prev = None
+    vals = []
+    maxval = 0
+    for i in range(0, len(a)):
+        if i % 2:
+            continue
+
+        maxval = max(maxval, abs(a[i]), abs(a[i + 1]))
+        if prev:
+            vals.append((a[i] - prev[0], a[i + 1] - prev[1]))
+        prev = a[i], a[i + 1]
+
+    assert len(vals) == len(a) / 2 - 1
+
+    vals = [(x[0] / float(maxval), x[1] / float(maxval)) for x in vals]
+
+    logging.info("Moves: %s", vals)
+    zoom = FIELD_WIDTH * scale
+    for item in vals:
+        plotter.line(item[0] * zoom, item[1] * zoom)
+
+    for item in reversed(vals):
+        plotter.line(-item[0] * zoom, item[1] * zoom)
+
+    for item in vals:
+        plotter.line(-item[0] * zoom, -item[1] * zoom)
+
+    for item in reversed(vals):
+        plotter.line(item[0] * zoom, -item[1] * zoom)
+
+    plotter.line(0.05 * zoom, 0)
+
+
+def angles_experiment():
+    parts = 2
+    for x in range(0, parts + 1):
+        movy = x * 1.0 / parts
+        plotter.line(1.0, movy)
+        plotter.move(-1.0, -movy)
+        logging.info("%s", movy)
+
+    for x in range(0, parts):
+        movx = x * 1.0 / parts
+        plotter.line(movx, 1.0)
+        plotter.move(-movx, -1.0)
+        logging.info("%s", movx)
+
+        """
+        path = 1000
+
+        spd_b = x * plotter.base_speed / parts
+        spd_a = plotter.base_speed - spd_b
+
+        angle = path * (1.0 + spd_b / spd_a)
+        logging.info("%s, %s, %s", angle, spd_a, spd_b)
+
+        plotter.motor_AB.angled(angle, spd_a, -spd_b)
+        plotter._compensate_wheels_backlash(-1)
+        plotter.motor_AB.angled(-angle, spd_a, -spd_b)
+        plotter._compensate_wheels_backlash(1)
+        """
+
+
+class MotorMock(EncodedMotor):
+    pass
+
+
+def get_hub_mock():
+    hub = HubMock()
+    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
+
+
+def interpret_command(cmd, plotter):
+    scale = 0.075
+    for c in cmd.lower():
+        if c == u'л':
+            plotter._transfer_to(-scale, 0)
+        elif c == u'п':
+            plotter._transfer_to(scale, 0)
+        elif c == u'н':
+            plotter._transfer_to(0, -scale)
+        elif c == u'в':
+            plotter._transfer_to(0, scale)
+        elif c == u'1':
+            plotter._tool_down()
+        elif c == u'0':
+            plotter._tool_up()
+        elif c == u' ':
+            pass
+        else:
+            logging.warning(u"Неизвестная команда: %s", c)
+
+
+if __name__ == '__main__':
+    logging.basicConfig(level=logging.DEBUG)
+    logging.getLogger('').setLevel(logging.DEBUG)
+
+    hub = MoveHub() if 1 else get_hub_mock()
+
+    plotter = Plotter(hub, 0.75)
+    FIELD_WIDTH = 0.9
+
+    try:
+        """
+        while True:
+            cmd = six.moves.input("программа> ").decode('utf8')
+            if not cmd.strip():
+                continue
+            plotter.initialize()
+            interpret_command(cmd, plotter)
+            plotter.finalize()
+        """
+
+        time.sleep(1)
+        plotter.initialize()
+
+        # snowflake(0.75)
+        # christmas_tree()
+        # square_spiral()
+        # lego(plotter, FIELD_WIDTH / 7.0)
+
+        # plotter._tool_down()
+        # angles_experiment()
+        # try_speeds()
+        # moves()
+        # triangle()
+        # square()
+        # cross()
+        # romb()
+        # circles()
+        # plotter.spiral(4, 0.02)
+        # plotter.rectangle(FIELD_WIDTH / 5.0, FIELD_WIDTH / 5.0, solid=True)
+
+        pass
+    finally:
+        plotter.finalize()

examples/sorter/__init__.py

@@ -0,0 +1,90 @@
+import logging
+
+from pylgbst.hub import MoveHub
+from pylgbst.peripherals import COLOR_YELLOW, COLOR_BLUE, COLOR_CYAN, COLOR_RED, COLOR_BLACK, COLORS
+
+
+class ColorSorter(MoveHub):
+    positions = [COLOR_YELLOW, COLOR_BLUE, COLOR_CYAN, COLOR_RED]
+
+    def __init__(self, connection=None):
+        super(ColorSorter, self).__init__(connection)
+        self.position = len(self.positions)
+        self.color = 0
+        self.distance = 10
+        self._last_wheel_dir = 1
+        self.vision_sensor.subscribe(self.on_color)
+        self.queue = [None for _ in range(0, 1)]
+
+    def on_color(self, colr, dist):
+        if colr not in (COLOR_BLACK,) or dist < 2.5:
+            logging.debug("%s %s", COLORS[colr], dist)
+        self.color = colr
+        self.distance = dist
+
+    def feed(self):
+        self.motor_A.angled(1080)  # 1080 was true
+
+    def move_to_bucket(self, color):
+        logging.info("Bucket: %s", COLORS[color])
+        if color in self.positions:
+            newpos = self.positions.index(color)
+        else:
+            newpos = len(self.positions)
+
+        if newpos == self.position:
+            return
+
+        offset = newpos - self.position
+        wheel_dir = offset / abs(offset)
+
+        # if wheel_dir != self._last_wheel_dir:
+        #    self.motor_B.angled(30 * wheel_dir)
+
+        self.motor_B.angled(offset * 600, 0.8)
+
+        self.position = newpos
+        self._last_wheel_dir = wheel_dir
+
+    def clear(self):
+        self.vision_sensor.unsubscribe(self.on_color)
+        self.move_to_bucket(COLOR_BLACK)
+        self.motor_AB.stop()
+
+    def tick(self):
+        res = False
+        item = (self.color, self.distance)  # read once
+
+        if item[1] <= 5.0:
+            logging.info("Detected: %s", COLORS[item[0]])
+            self.queue.append(item)
+            res = True
+        else:
+            self.queue.append(None)
+
+        logging.debug("%s", [COLORS[x[0]] if x else None for x in self.queue])
+
+        last = self.queue.pop(0)
+        if last:
+            self.move_to_bucket(last[0])
+            res = True
+
+        self.feed()
+        return res
+
+
+if __name__ == '__main__':
+    logging.basicConfig(level=logging.INFO)
+
+    sorter = ColorSorter()
+    empty = 0
+    try:
+        while True:
+            empty += 1
+            if sorter.tick():
+                empty = 0
+            elif empty > 20:
+                break
+
+    finally:
+        sorter.clear()

examples/tracker/__init__.py

@@ -0,0 +1,218 @@
+import json
+import logging
+import os
+import time
+import traceback
+from threading import Thread
+
+import cv2
+import imutils as imutils
+from matplotlib import pyplot
+
+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')
+smile_cascade = cv2.CascadeClassifier(cascades_dir + '/haarcascade_smile.xml')
+
+LINE_THICKNESS = 2
+
+
+class FaceTracker(MoveHub):
+    def __init__(self, connection=None):
+        super(FaceTracker, self).__init__(connection)
+        self._is_smile_on = False
+        self.cur_img = None
+        self.cur_face = None
+        self.cur_smile = None
+        self.smile_counter = 0
+
+    def capture(self):
+        logging.info("Starting cam...")
+        cap = cv2.VideoCapture(0)
+        cap.set(cv2.CAP_PROP_FRAME_WIDTH, 1920 / 2)
+        cap.set(cv2.CAP_PROP_FRAME_HEIGHT, 1080 / 2)
+        # cap.set(cv2.CAP_PROP_AUTO_EXPOSURE, 1)
+        # cap.set(cv2.CAP_PROP_XI_AUTO_WB, 1)
+        # cap.set(cv2.CAP_PROP_XI_AEAG, 1)
+
+        size = (int(cap.get(cv2.CAP_PROP_FRAME_WIDTH)), int(cap.get(cv2.CAP_PROP_FRAME_HEIGHT)))
+
+        fourcc = cv2.VideoWriter_fourcc(*'XVID')
+        fps = cap.get(cv2.CAP_PROP_FPS)
+        video = cv2.VideoWriter('output_%d.avi' % int(time.time()), fourcc, fps, size)
+
+        try:
+            while True:
+                flag, img = cap.read()
+                if self.cur_face is not None:
+                    (x, y, w, h) = self.cur_face
+                    if LINE_THICKNESS:
+                        cv2.rectangle(img, (x, y), (x + w, y + h), (0, 0, 255,), LINE_THICKNESS)
+                video.write(img)
+                self.cur_img = cv2.cvtColor(img, cv2.COLOR_BGR2RGB)
+                logging.debug("Got frame")
+        finally:
+            logging.info("Releasing cam...")
+            cap.release()
+            video.release()
+
+    def _find_face(self):
+        bodies, rejects, weights = face_cascade.detectMultiScale3(self.cur_img, 1.5, 5, outputRejectLevels=True)
+        if len(bodies):
+            logging.debug("Bodies: %s / Weights: %s", bodies, weights)
+        else:
+            return None
+
+        items = []
+        for n in range(0, len(bodies)):
+            items.append((bodies[n], weights[n]))
+
+        self.cur_face = None
+        return self._reduce(bodies)
+
+    def _reduce(self, values):
+        res = None
+        for x, y, w, h in values:
+            if res is None:
+                res = (x, y, w, h)
+            else:
+                new_xy = (min(x, res[0]), min(y, res[1]))
+                res = new_xy + (max(x + w, res[0] + res[2]) - new_xy[0], max(y + h, res[1] + res[3]) - new_xy[1])
+        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]
+            smile = self._reduce(smile_cascade.detectMultiScale(roi_color, 1.5, 20))
+        else:
+            smile = None
+
+        if not smile:
+            self.cur_smile = None
+            self.smile_counter -= 1
+        else:
+            (ex, ey, ew, eh) = smile
+            self.cur_smile = (ex, ey, ew, eh)
+            if LINE_THICKNESS:
+                cv2.rectangle(roi_color, (ex, ey), (ex + ew, ey + eh), (0, 255, 0), LINE_THICKNESS)
+            self.smile_counter += 1
+
+        logging.info("Smile counter: %s", self.smile_counter)
+        if self.smile_counter > 2:
+            self.smile_counter = 2
+            self._smile(True)
+        if self.smile_counter < 0:
+            self.smile_counter = 0
+            self._smile(False)
+
+    def _smile(self, on=True):
+        if on and not self._is_smile_on:
+            self._is_smile_on = True
+            self.motor_B.angled(-90, 0.5)
+            self.led.set_color(COLOR_RED)
+
+        if not on and self._is_smile_on:
+            self._is_smile_on = False
+            self.motor_B.angled(90, 0.5)
+
+    def _find_color(self):
+        # from https://www.pyimagesearch.com/2015/09/14/ball-tracking-with-opencv/
+        # and https://thecodacus.com/opencv-object-tracking-colour-detection-python/#.W2DHFd_IQsM
+
+        blurred = cv2.GaussianBlur(self.cur_img, (11, 11), 0)
+        hsv = cv2.cvtColor(blurred, cv2.COLOR_BGR2HSV)
+
+        try:
+            # having color values in file allows finding values easier
+            with open(os.path.join(os.path.dirname(__file__), "color.json")) as fhd:
+                data = json.loads(fhd.read())
+                lower = tuple(data[0])
+                upper = tuple(data[1])
+        except BaseException:
+            logging.debug("%s", traceback.format_exc())
+            lower = (100, 100, 100,)
+            upper = (130, 255, 255,)
+        mask = cv2.inRange(hsv, lower, upper)
+        mask = cv2.erode(mask, None, iterations=5)
+        mask = cv2.dilate(mask, None, iterations=5)
+
+        # if not (int(time.time()) % 2):
+        #    self.cur_img = mask
+
+        ret, thresh = cv2.threshold(mask, 20, 255, 0)
+        cnts = cv2.findContours(thresh, cv2.RETR_EXTERNAL, cv2.CHAIN_APPROX_NONE)
+        cnts = cnts[0] if imutils.is_cv2() else cnts[1]
+
+        return self._reduce([cv2.boundingRect(c) for c in cnts])
+
+    def _auto_pan(self, cur_face):
+        (x, y, w, h) = cur_face
+        height, width, channels = self.cur_img.shape
+
+        cam_center = (width / 2, height / 2)
+        face_center = (x + w / 2, y + h / 2)
+
+        horz = 1.5 * (face_center[0] - cam_center[0]) / float(width)
+        vert = 0.7 * (face_center[1] - cam_center[1]) / float(height)
+
+        logging.info("Horiz %.3f, vert %3f", horz, vert)
+        if abs(horz) < 0.1:
+            horz = 0
+        if abs(vert) < 0.15:
+            vert = 0
+
+        self.motor_external.start_power(horz)
+        self.motor_A.start_power(-vert)
+
+    def main(self):
+        thr = Thread(target=self.capture)
+        thr.setDaemon(True)
+        thr.start()
+
+        while self.cur_img is None:
+            pass
+
+        plt = pyplot.imshow(self.cur_img)
+        pyplot.tight_layout()
+        pyplot.ion()
+        pyplot.show()
+
+        try:
+            while thr.isAlive() and self.connection.is_alive():
+                self._process_picture(plt)
+        finally:
+            self._smile(False)
+
+    def _process_picture(self, plt):
+        self.cur_face = self._find_face()
+        # 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:
+                self.led.set_color(COLOR_BLUE)
+        else:
+            if not self._is_smile_on:
+                self.led.set_color(COLOR_YELLOW)
+
+            self._auto_pan(self.cur_face)
+            self._find_smile(self.cur_face)
+        plt.set_array(self.cur_img)
+        logging.debug("Updated frame")
+        pyplot.pause(0.02)
+
+
+if __name__ == '__main__':
+    logging.basicConfig(level=logging.INFO)
+
+    try:
+        hub = FaceTracker()
+        hub.main()
+    finally:
+        pass
+        # conn.disconnect()

examples/tracker/color.json

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

examples/vernie/__init__.py

@@ -3,12 +3,17 @@ import hashlib
 import os
 import re
 import subprocess
+import time
+
+from pylgbst import *
+from pylgbst.hub import MoveHub
 
 try:
     import gtts
 
 
     def say(text):
+        print("%s" % text)
         if isinstance(text, str):
             text = text.decode("utf-8")
         md5 = hashlib.md5(text.encode('utf-8')).hexdigest()
@@ -23,49 +28,47 @@ try:
 
         with open(os.devnull, 'w') as fnull:
             subprocess.call("mplayer %s" % fname, shell=True, stderr=fnull, stdout=fnull)
-except:
+except BaseException:
     def say(text):
-        sys.stdout.write("%s\n", text)
-
-from pylgbst import *
+        print("%s" % text)
 
 forward = FORWARD = right = RIGHT = 1
 backward = BACKWARD = left = LEFT = -1
 straight = STRAIGHT = 0
 
+SPEECH_LANG_MAP = {
+    'en': {
+        'ready': "Vernie the Robot is ready.",
+        "commands help": "Available commands are: "
+                         "forward, backward, turn left, turn right, "
+                         "head left, head right, head straight, shot and say",
+        "finished": "Thank you! Robot is now turning off",
+        "text is empty": "Please, enter not empty text to say!"
+    },
+    "ru": {
+        "ready": "Робот Веернии готов к работе",
+        "type commands": "печатайте команды",
+        "ok": "хорошо",
+        "commands help": "Доступные команды это: вперёд, назад, влево, вправо, "
+                         "голову влево, голову вправо, голову прямо, выстрел, скажи",
+        "finished": "Робот завершает работу. Спасибо!",
+        "commands from file": "Исполняю команды из файла",
+        "fire": "Выстрел!",
+        "text is empty": "Пожалуйста, наберите не пустой текст!"
+    }
+}
+
+VERNIE_TO_MOTOR_DEGREES = 2.6
+VERNIE_SINGLE_MOVE = 430
+
 
 class Vernie(MoveHub):
-    SPEECH_LANG_MAP = {
-        'en': {
-            'ready': "Vernie the Robot is ready.",
-            "commands help": "Available commands are: "
-                             "forward, backward, turn left, turn right, "
-                             "head left, head right, head straight and say",
-            "finished": "Thank you! Robot is now turning off"
-        },
-        "ru": {
-            "ready": "Робот Веернии 01 готов к работе",
-            "type commands": "печатайте команды",
-            "ok": "хорошо",
-            "commands help": "Доступные команды это: вперёд, назад, поворот влево, поворот вправо, "
-                             "голову влево, голову вправо, голову прямо, скажи",
-            "Finished": "Робот завершает работу. Спасибо!",
-            "commands from file": "Исполняю команды из файла",
-        }
-    }
-
     def __init__(self, language='en'):
+        super(Vernie, self).__init__()
         self.language = language
-        try:
-            conn = DebugServerConnection()
-        except BaseException:
-            logging.debug("Failed to use debug server: %s", traceback.format_exc())
-            conn = BLEConnection().connect()
-
-        super(Vernie, self).__init__(conn)
 
         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)
@@ -74,13 +77,13 @@ class Vernie(MoveHub):
         self._head_position = 0
         self.motor_external.subscribe(self._external_motor_data)
 
-        # self._reset_head() FIXME: restore it
-        # self.say("ready")
+        self._reset_head()
+        self.say("ready")
         time.sleep(1)
 
     def say(self, phrase):
-        if phrase in self.SPEECH_LANG_MAP[self.language]:
-            phrase = self.SPEECH_LANG_MAP[self.language][phrase]
+        if phrase in SPEECH_LANG_MAP[self.language]:
+            phrase = SPEECH_LANG_MAP[self.language][phrase]
         say(phrase)
 
     def _external_motor_data(self, data):
@@ -99,18 +102,23 @@ class Vernie(MoveHub):
         self.motor_external.angled(direction * angle, speed)
 
     def turn(self, direction, degrees=90, speed=0.3):
-        self.head(STRAIGHT, speed=1)
-        self.head(direction, 35, 1)
-        self.motor_AB.angled(225 * degrees / 90, speed * direction, -speed * direction)
-        self.head(STRAIGHT, speed=1)
-
-    def move(self, direction, distance=1, speed=0.3):
         self.head(STRAIGHT, speed=0.5)
-        self.motor_AB.angled(distance * 450, speed * direction, speed * direction)
+        self.head(direction, 35, 1)
+        self.motor_AB.angled(int(VERNIE_TO_MOTOR_DEGREES * degrees), speed * direction, -speed * direction)
+        self.head(STRAIGHT, speed=0.5)
+
+    def move(self, direction, distance=1, speed=0.2):
+        self.head(STRAIGHT, speed=0.5)
+        self.motor_AB.angled(distance * VERNIE_SINGLE_MOVE, speed * direction, speed * direction)
+
+    def shot(self):
+        self.motor_external.timed(0.5)
+        self.head(STRAIGHT)
+        self.head(STRAIGHT)
 
     def interpret_command(self, cmd, confirm):
         cmd = cmd.strip().lower().split(' ')
-        if cmd[0] in ("head", "голова", "голова"):
+        if cmd[0] in ("head", "голова", "голову"):
             if cmd[-1] in ("right", "вправо", "направо"):
                 confirm(cmd)
                 self.head(RIGHT)
@@ -121,21 +129,27 @@ class Vernie(MoveHub):
                 confirm(cmd)
                 self.head(STRAIGHT)
         elif cmd[0] in ("say", "скажи", "сказать"):
+            if not cmd[1:]:
+                self.say("text is empty")
+                return
             say(' '.join(cmd[1:]))
-        elif cmd[0] in ("end", "конец"):
+        elif cmd[0] in ("fire", "shot", "огонь", "выстрел"):
+            say("fire")
+            self.shot()
+        elif cmd[0] in ("end", "finish", "конец", "стоп"):
             self.say("finished")
             raise KeyboardInterrupt()
         elif cmd[0] in ("forward", "вперёд", "вперед"):
             try:
                 dist = int(cmd[-1])
-            except:
+            except BaseException:
                 dist = 1
             confirm(cmd)
             self.move(FORWARD, distance=dist)
         elif cmd[0] in ("backward", "назад"):
             try:
                 dist = int(cmd[-1])
-            except:
+            except BaseException:
                 dist = 1
             confirm(cmd)
             self.move(BACKWARD, distance=dist)
@@ -149,9 +163,12 @@ class Vernie(MoveHub):
             else:
                 confirm(cmd)
                 self.turn(RIGHT, degrees=180)
-        else:
+        elif cmd[0] in ("right", "вправо", "направо"):
+            confirm(cmd)
+            self.turn(RIGHT)
+        elif cmd[0] in ("left", "влево", "налево"):
+            confirm(cmd)
+            self.turn(LEFT)
+        elif cmd[0]:
             self.say("Unknown command")
             self.say("commands help")
-
-# TODO: disable motors if state is not up
-# TODO: stop motors on bump?

examples/vernie/android_remote.py

@@ -0,0 +1,92 @@
+"""
+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 field.
+Specify port there as 8999, and enable streaming. Then run this script on computer.
+"""
+import logging
+import socket
+import time
+
+from examples.vernie import Vernie
+from pylgbst.peripherals import VisionSensor
+
+host = ''
+port = 8999
+running = True
+
+
+def on_btn(pressed):
+    global running
+    if pressed:
+        running = False
+
+
+def decode_xml(msg):
+    parts = msg.split("</")
+    xxx = float(parts[1][parts[1].rfind('>') + 1:])
+    yyy = float(parts[2][parts[2].rfind('>') + 1:])
+    zzz = float(parts[3][parts[3].rfind('>') + 1:])
+    return ranged(xxx), ranged(yyy), ranged(zzz)
+
+
+def ranged(param):
+    return float(param / 10)
+
+
+front_distance = 0
+
+
+def on_distance(distance):
+    logging.info("Distance %s", distance)
+    global front_distance
+    front_distance = distance
+
+
+udp_sock = socket.socket(socket.AF_INET, socket.SOCK_DGRAM)
+udp_sock.setsockopt(socket.SOL_SOCKET, socket.SO_REUSEADDR, 1)
+udp_sock.setsockopt(socket.SOL_SOCKET, socket.SO_BROADCAST, 1)
+udp_sock.settimeout(0)
+
+logging.basicConfig(level=logging.INFO)
+robot = Vernie()
+robot.button.subscribe(on_btn)
+robot.motor_AB.stop()
+
+robot.vision_sensor.subscribe(on_distance, VisionSensor.DISTANCE_INCHES)
+try:
+    udp_sock.bind((host, port))
+    time.sleep(1)
+
+    while running:
+        message = ""
+        while True:
+            try:
+                data = udp_sock.recv(8192)
+                message = data
+            except KeyboardInterrupt:
+                raise
+            except BaseException:
+                break
+
+        if not message:
+            time.sleep(0.1)
+            continue
+
+        messageString = message.decode("utf-8")
+        a, b, c = decode_xml(messageString)
+        divider = 2.0 if c > 0 else -2.0
+
+        if 0 < front_distance < 9 and c > 0:
+            logging.info("Something in front of Vernie [%s]!", front_distance)
+            c = 0
+
+        sa = round(c + b / divider, 1)
+        sb = round(c - b / divider, 1)
+        logging.info("SpeedA=%s, SpeedB=%s", sa, sb)
+        robot.motor_AB.start_speed(sa, sb)
+        # time.sleep(0.5)
+finally:
+    robot.motor_AB.stop()
+    udp_sock.close()

examples/vernie/go_towards_light.py

@@ -0,0 +1,69 @@
+import logging
+
+from examples.vernie import Vernie
+from pylgbst.peripherals import VisionSensor
+
+logging.basicConfig(level=logging.INFO)
+
+robot = Vernie()
+running = True
+criterion = min
+
+cur_luminosity = 0
+
+
+def on_change_lum(lumn, unknown):
+    del unknown
+    global cur_luminosity
+    cur_luminosity = lumn
+
+
+lum_values = {}
+
+
+def on_btn(pressed):
+    global running
+    if pressed:
+        running = False
+
+
+def on_turn(angl):
+    lum_values[angl] = cur_luminosity
+
+
+robot.button.subscribe(on_btn)
+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?
+
+while running:
+    # turn around, measuring luminosity
+    lum_values = {}
+    robot.turn(RIGHT, degrees=360, speed=0.2)
+
+    # get max luminosity angle
+    amin = min(lum_values.keys())
+    lmax = max(lum_values.values())
+    almax = amin
+    for almax in lum_values:
+        if lum_values[almax] == lmax:
+            break
+
+    angle = int((almax - amin) / VERNIE_TO_MOTOR_DEGREES)
+    logging.info("Angle to brightest %.3f is %s", lmax, angle)
+
+    # turn towards light
+    if angle > 180:
+        robot.turn(LEFT, degrees=360 - angle)
+    else:
+        robot.turn(RIGHT, degrees=angle)
+
+    # Now let's move until luminosity changes
+    lum = cur_luminosity
+    while cur_luminosity >= lum:
+        logging.info("Luminosity is %.3f, moving towards it", cur_luminosity)
+        robot.move(FORWARD, 1)
+
+robot.vision_sensor.unsubscribe(on_change_lum)
+robot.button.unsubscribe(on_btn)

examples/vernie/playmat_path.py

@@ -1,4 +1,4 @@
-from vernie import *
+from . import *
 
 robot = Vernie()
 

examples/vernie/read_typed_commands.py

@@ -1,15 +1,17 @@
-from vernie import *
+import six
+
+from . import *
 
 robot = Vernie()
 
 robot.say('type commands')
 
 
-def confirmation(cmd):
+def confirmation(_):
     robot.say("ok")
 
 
 while True:
     # noinspection PyUnresolvedReferences
-    cmd = six.moves.input("COMMAND >")
+    cmd = six.moves.input("> ")
     robot.interpret_command(cmd, confirmation)

examples/vernie/run_away_game.py

@@ -1,18 +1,18 @@
+from pylgbst.peripherals import COLOR_GREEN, COLOR_NONE
 from vernie import *
 
 robot = Vernie()
 running = True
 
-robot.say("Place your hand in front of sensor")
-
 
 def callback(color, distance):
+    robot.led.set_color(color)
     speed = (10 - distance + 1) / 10.0
     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.say("Place your hand in front of sensor")
+        robot.motor_AB.timed(secs / 1, -speed)
+        robot.say("Ouch")
 
 
 def on_btn(pressed):
@@ -21,14 +21,13 @@ def on_btn(pressed):
         running = False
 
 
-robot.button.subscribe(on_btn)
-robot.color_distance_sensor.subscribe(callback)
 robot.led.set_color(COLOR_GREEN)
+robot.button.subscribe(on_btn)
+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)
-time.sleep(10)  # let color change

examples/vernie/run_commands_file.py

@@ -1,15 +1,17 @@
-from vernie import *
+import sys
+
+from . import *
 
 robot = Vernie()
 
 robot.say("commands from file")
 
 
-def confirmation(cmd):
-    robot.say(cmd[0])
+def confirmation(command):
+    robot.say(command[0])
 
 
-with open("vernie.commands") as fhd:
+with open(os.path.join(os.path.dirname(__file__), "vernie.commands")) as fhd:
     for cmd in fhd.readlines():
         sys.stdout.write("%s" % cmd)
         robot.interpret_command(cmd, confirmation)

examples/vernie/vernie.commands

@@ -16,4 +16,6 @@ TURN RIGHT
 FORWARD 3
 TURN LEFT
 TURN LEFT
-BACKWARD 2
+BACKWARD 2
+SHOT
+END

pylgbst/__init__.py

@@ -1,168 +1,86 @@
-from pylgbst.comms import *
-from pylgbst.constants import *
-from pylgbst.peripherals import *
+import logging
+import traceback
 
-log = logging.getLogger('movehub')
+from pylgbst.comms import DebugServer
+
+log = logging.getLogger('pylgbst')
 
 
-class MoveHub(object):
+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, hub_name)
+
+
+def get_connection_gattool(controller='hci0', hub_mac=None, hub_name=None):
+    from pylgbst.comms.cpygatt import GattoolConnection
+
+    return GattoolConnection(controller).connect(hub_mac, hub_name)
+
+
+def get_connection_gatt(controller='hci0', hub_mac=None, hub_name=None):
+    from pylgbst.comms.cgatt import GattConnection
+
+    return GattConnection(controller).connect(hub_mac, hub_name)
+
+
+def get_connection_gattlib(controller='hci0', hub_mac=None, hub_name=None):
+    from pylgbst.comms.cgattlib import GattLibConnection
+
+    return GattLibConnection(controller).connect(hub_mac, hub_name)
+
+
+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):
     """
-    :type connection: pylgbst.comms.Connection
-    :type devices: dict[int,Peripheral]
-    :type led: LED
-    :type tilt_sensor: TiltSensor
-    :type button: Button
-    :type battery: Battery
-    :type color_distance_sensor: pylgbst.peripherals.ColorDistanceSensor
-    :type motor_external: EncodedMotor
-    :type port_C: Peripheral
-    :type port_D: Peripheral
-    :type motor_A: EncodedMotor
-    :type motor_B: EncodedMotor
-    :type motor_AB: EncodedMotor
+    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
 
-    def __init__(self, connection=None):
-        if not connection:
-            connection = BLEConnection()
+    return BleakDriver(hub_mac, hub_name)
 
-        self.connection = connection
-        self.devices = {}
 
-        # shorthand fields
-        self.button = Button(self)
-        self.led = None
-        self.battery = None
-        self.motor_A = None
-        self.motor_B = None
-        self.motor_AB = None
-        self.tilt_sensor = None
-        self.color_distance_sensor = None
-        self.motor_external = None
-        self.port_C = None
-        self.port_D = None
+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,
+    ]
 
-        self.connection.set_notify_handler(self._notify)
+    conn = None
+    for fn in fns:
+        try:
+            logging.info("Trying %s", fn.__name__)
+            return fn(controller, hub_mac, hub_name)
+        except KeyboardInterrupt:
+            raise
+        except BaseException:
+            logging.debug("Failed: %s", traceback.format_exc())
 
-        self._wait_for_devices()
+    if conn is None:
+        raise Exception("Failed to autodetect connection, make sure you have installed prerequisites")
 
-    def _wait_for_devices(self):
-        self.connection.write(ENABLE_NOTIFICATIONS_HANDLE, ENABLE_NOTIFICATIONS_VALUE)
+    logging.info("Succeeded with %s", conn.__class__.__name__)
+    return conn
 
-        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)
-            if None not in builtin_devices:
-                return
-            log.debug("Waiting for builtin devices to appear: %s", builtin_devices)
-            time.sleep(1)
-        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
-
-        data = data[3:]
-        log.debug("Notification on %s: %s", handle, str2hex(orig))
-
-        msg_type = get_byte(data, 2)
-
-        if msg_type == MSG_PORT_INFO:
-            self._handle_port_info(data)
-        elif msg_type == MSG_PORT_STATUS:
-            self._handle_port_status(data)
-        elif msg_type == MSG_SENSOR_DATA:
-            self._handle_sensor_data(data)
-        elif msg_type == MSG_SENSOR_SUBSCRIBE_ACK:
-            log.debug("Sensor subscribe ack on port %s", PORTS[get_byte(data, 3)])
-        elif msg_type == MSG_PORT_CMD_ERROR:
-            log.warning("Command error: %s", str2hex(data[3:]))
-        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):
-        if get_byte(data, 3) == 2:
-            self.button.handle_port_data(data)
-        else:
-            log.warning("Unhandled device info: %s", str2hex(data))
-
-    def _handle_sensor_data(self, data):
-        port = get_byte(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 = get_byte(data, 3)
-        status = get_byte(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])
-        else:
-            log.warning("Unhandled status value: 0x%x", status)
-
-    def _handle_port_info(self, data):
-        port = get_byte(data, 3)
-        dev_type = get_byte(data, 5)
-
-        if port in PORTS and dev_type in DEVICE_TYPES:
-            log.debug("Device %s at port %s", DEVICE_TYPES[dev_type], PORTS[port])
-        else:
-            log.warning("Device 0x%x at 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_BATTERY:
-            self.devices[port] = Battery(self, port)
-        else:
-            log.debug("Unhandled peripheral type 0x%x on port 0x%x", dev_type, port)
-            self.devices[port] = Peripheral(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_BATTERY:
-            self.battery = self.devices[port]
-        else:
-            log.debug("Unhandled port: %s", PORTS[port])
-
-    def shutdown(self):
-        cmd = pack("<B", PACKET_VER) + pack("<B", MSG_DEVICE_SHUTDOWN)
-        self.connection.write(MOVE_HUB_HARDWARE_HANDLE, pack("<B", len(cmd) + 1) + cmd)
+def start_debug_server(iface="hci0", port=9090):
+    server = DebugServer(get_connection_auto(iface))
+    try:
+        server.start(port)
+    finally:
+        server.connection.disconnect()

pylgbst/comms/__init__.py

@@ -5,71 +5,35 @@ import binascii
 import json
 import logging
 import socket
-import sys
 import traceback
 from abc import abstractmethod
 from binascii import unhexlify
-from gattlib import DiscoveryService, GATTRequester
 from threading import Thread
 
-from six.moves import queue
-
-from pylgbst.constants import LEGO_MOVE_HUB, MSG_DEVICE_SHUTDOWN
+from pylgbst.messages import MsgHubAction
+from pylgbst.utilities import str2hex
 
 log = logging.getLogger('comms')
 
+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'
 
-def str2hex(data):  # TODO: eliminate it
-    return binascii.hexlify(data).decode("utf8")
-
-
-if sys.version_info[0] == 2:
-    def get_byte(seq, index):
-        return ord(seq[index])
-else:
-    def get_byte(seq, index):
-        return seq[index]
-
-
-# noinspection PyMethodOverriding
-class Requester(GATTRequester):
-    """
-    Wrapper to access `on_notification` capability of GATT
-    Set "notification_sink" field to a callable that will handle incoming data
-    """
-
-    def __init__(self, p_object, *args, **kwargs):
-        super(Requester, self).__init__(p_object, *args, **kwargs)
-        self.notification_sink = None
-
-        # noinspection PyUnresolvedReferences
-        self._notify_queue = queue.Queue()  # this queue is to minimize time spent in gattlib C code
-        thr = Thread(target=self._dispatch_notifications)
-        thr.setDaemon(True)
-        thr.setName("Notify queue dispatcher")
-        thr.start()
-
-    def on_notification(self, handle, data):
-        # log.debug("requester notified, sink: %s", self.notification_sink)
-        self._notify_queue.put((handle, data))
-
-    def on_indication(self, handle, data):
-        log.debug("Indication on handle %s: %s", handle, str2hex(data))
-
-    def _dispatch_notifications(self):
-        while True:
-            handle, data = self._notify_queue.get()
-            if self.notification_sink:
-                try:
-                    self.notification_sink(handle, data)
-                except BaseException:
-                    log.warning("Data was: %s", str2hex(data))
-                    log.warning("Failed to dispatch notification: %s", traceback.format_exc())
-            else:
-                log.warning("Dropped notification %s: %s", handle, str2hex(data))
+MOVE_HUB_HARDWARE_HANDLE = 0x0E
 
 
 class Connection(object):
+    def connect(self, hub_mac=None):
+        pass
+
+    @abstractmethod
+    def is_alive(self):
+        pass
+
+    def disconnect(self):
+        pass
+
     @abstractmethod
     def write(self, handle, data):
         pass
@@ -78,45 +42,24 @@ class Connection(object):
     def set_notify_handler(self, handler):
         pass
 
+    def enable_notifications(self):
+        self.write(ENABLE_NOTIFICATIONS_HANDLE, ENABLE_NOTIFICATIONS_VALUE)
 
-class BLEConnection(Connection):
-    """
-    Main transport class, uses real Bluetooth LE connection.
-    Loops with timeout of 1 seconds to find device named "Lego MOVE Hub"
+    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
 
-    :type requester: Requester
-    """
+            if matched:
+                log.info("Found %s at %s", dev_name, address)
 
-    def __init__(self):
-        super(BLEConnection, self).__init__()
-        self.requester = None
-
-    def connect(self, bt_iface_name='hci0'):
-        service = DiscoveryService(bt_iface_name)
-
-        while not self.requester:
-            log.info("Discovering devices using %s...", bt_iface_name)
-            devices = service.discover(1)
-            log.debug("Devices: %s", devices)
-
-            for address, name in devices.items():
-                if name == LEGO_MOVE_HUB:
-                    logging.info("Found %s at %s", name, address)
-                    self.requester = Requester(address, True, bt_iface_name)
-                    break
-
-        return self
-
-    def set_notify_handler(self, handler):
-        if self.requester:
-            log.debug("Setting notification handler: %s", handler)
-            self.requester.notification_sink = handler
-        else:
-            raise RuntimeError("No requester available")
-
-    def write(self, handle, data):
-        log.debug("Writing to %s: %s", handle, str2hex(data))
-        return self.requester.write_by_handle(handle, data)
+        return matched
 
 
 class DebugServer(object):
@@ -125,13 +68,13 @@ class DebugServer(object):
     It holds BLE connection to Move Hub, so no need to re-start it every time
     Usage: DebugServer(BLEConnection().connect()).start()
 
-    :type ble: BLEConnection
+    :type connection: BLEConnection
     """
 
-    def __init__(self, ble_trans):
+    def __init__(self, connection):
         self._running = False
         self.sock = socket.socket()
-        self.ble = ble_trans
+        self.connection = connection
 
     def start(self, port=9090):
         self.sock.bind(('', port))
@@ -139,11 +82,11 @@ class DebugServer(object):
 
         self._running = True
         while self._running:
-            log.info("Accepting connections at %s", port)
+            log.info("Accepting MoveHub debug connections at %s", port)
             conn, addr = self.sock.accept()
             if not self._running:
                 raise KeyboardInterrupt("Shutdown")
-            self.ble.requester.notification_sink = lambda x, y: self._notify(conn, x, y)
+            self.connection.set_notify_handler(lambda x, y: self._notify(conn, x, y))
             try:
                 self._handle_conn(conn)
             except KeyboardInterrupt:
@@ -151,14 +94,14 @@ class DebugServer(object):
             except BaseException:
                 log.error("Problem handling incoming connection: %s", traceback.format_exc())
             finally:
-                self.ble.requester.notification_sink = self._notify_dummy
+                self.connection.set_notify_handler(self._notify_dummy)
                 conn.close()
 
     def __del__(self):
         self.sock.close()
 
     def _notify_dummy(self, handle, data):
-        log.debug("Dropped notification from handle %s: %s", handle, binascii.hexlify(data.strip()))
+        log.debug("Dropped notification from handle %s: %s", handle, binascii.hexlify(data))
         self._check_shutdown(data)
 
     def _notify(self, conn, handle, data):
@@ -174,7 +117,7 @@ class DebugServer(object):
         self._check_shutdown(data)
 
     def _check_shutdown(self, data):
-        if get_byte(data, 5) == MSG_DEVICE_SHUTDOWN:
+        if data[5] == MsgHubAction.TYPE:
             log.warning("Device shutdown")
             self._running = False
 
@@ -196,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:
@@ -206,7 +149,7 @@ class DebugServer(object):
 
     def _handle_cmd(self, cmd):
         if cmd['type'] == 'write':
-            self.ble.write(cmd['handle'], unhexlify(cmd['data']))
+            self.connection.write(cmd['handle'], unhexlify(cmd['data']))
         else:
             raise ValueError("Unhandled cmd: %s", cmd)
 
@@ -271,9 +214,5 @@ class DebugServerConnection(Connection):
     def set_notify_handler(self, handler):
         self.notify_handler = handler
 
-
-def start_debug_server(iface="hci0", port=9090):
-    ble = BLEConnection()
-    ble.connect(iface)
-    server = DebugServer(ble)
-    server.start(port)
+    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

@@ -0,0 +1,127 @@
+import logging
+import re
+import threading
+from time import sleep
+
+import gatt
+
+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')
+
+
+class CustomDevice(gatt.Device, object):
+    def __init__(self, mac_address, manager):
+        gatt.Device.__init__(self, mac_address=mac_address, manager=manager)
+        self._notify_callback = lambda hnd, val: None
+        self._handle = None
+
+    def connect(self):
+        gatt.Device.connect(self)
+        log.info("Waiting for device connection...")
+        while self._handle is None:
+            log.debug("Sleeping...")
+            sleep(1)
+
+        if isinstance(self._handle, BaseException):
+            exc = self._handle
+            self._handle = None
+            raise exc
+
+    def write(self, data):
+        log.debug("Writing to handle: %s", str2hex(data))
+        return self._handle.write_value(data)
+
+    def enable_notifications(self):
+        log.debug('Enable Notifications...')
+        self._handle.enable_notifications()
+
+    def set_notific_handler(self, func_hnd):
+        self._notify_callback = func_hnd
+
+    def services_resolved(self):
+        log.debug('Getting MoveHub services and characteristics...')
+        gatt.Device.services_resolved(self)
+        log.debug("[%s] Resolved services", self.mac_address)
+        for service in self.services:
+            log.debug("[%s]  Service [%s]", self.mac_address, service.uuid)
+            for characteristic in service.characteristics:
+                log.debug("[%s]    Characteristic [%s]", self.mac_address, characteristic.uuid)
+                if service.uuid == MOVE_HUB_HW_UUID_SERV and characteristic.uuid == MOVE_HUB_HW_UUID_CHAR:
+                    log.debug('MoveHub characteristic found')
+                    self._handle = characteristic
+
+        if self._handle is None:
+            self.manager.stop()
+            self._handle = RuntimeError("Failed to obtain MoveHub handle")
+
+    def characteristic_value_updated(self, characteristic, value):
+        value = self._fix_weird_bug(value)
+        log.debug('Notification in GattDevice: %s', str2hex(value))
+        self._notify_callback(MOVE_HUB_HARDWARE_HANDLE, value)
+
+    def _fix_weird_bug(self, value):
+        if isinstance(value, str) and "dbus.Array" in value:  # weird bug from gatt on my Ubuntu 16.04!
+            log.debug("Fixing broken notify string: %s", value)
+            return ''.join([chr(int(x.group(1))) for x in re.finditer(r"dbus.Byte\((\d+)\)", value)])
+
+        return value
+
+
+class GattConnection(Connection):
+    """
+    :type _device: CustomDevice
+    """
+
+    def __init__(self, bt_iface_name='hci0'):
+        super(GattConnection, self).__init__()
+        self._device = None
+        self._iface = bt_iface_name
+        try:
+            self._manager = gatt.DeviceManager(adapter_name=self._iface)
+        except TypeError:
+            raise NotImplementedError("Gatt is not implemented for this platform")
+
+        self._manager_thread = threading.Thread(target=self._manager.run)
+        self._manager_thread.setDaemon(True)
+        log.debug('Starting DeviceManager...')
+
+    def connect(self, hub_mac=None, hub_name=None):
+        self._manager_thread.start()
+        self._manager.start_discovery()
+
+        while not self._device:
+            log.info("Discovering devices...")
+            devices = self._manager.devices()
+            log.debug("Devices: %s", devices)
+
+            for dev in devices:
+                address = dev.mac_address
+                name = dev.alias()
+                if self._is_device_matched(address, name, hub_mac, hub_name):
+                    self._device = CustomDevice(address, self._manager)
+                    break
+
+            if not self._device:
+                sleep(1)
+
+        self._device.connect()
+        return self
+
+    def disconnect(self):
+        self._manager.stop()
+        self._device.disconnect()
+
+    def write(self, handle, data):
+        self._device.write(data)
+
+    def set_notify_handler(self, handler):
+        self._device.set_notific_handler(handler)
+
+    def enable_notifications(self):
+        self._device.enable_notifications()
+
+    def is_alive(self):
+        return self._manager_thread.isAlive()

pylgbst/comms/cgattlib.py

@@ -0,0 +1,94 @@
+# noinspection PyMethodOverriding
+import logging
+import traceback
+from threading import Thread
+
+from gattlib import DiscoveryService, GATTRequester
+
+from pylgbst.comms import Connection
+from pylgbst.utilities import queue, str2hex
+
+log = logging.getLogger('comms-gattlib')
+
+
+class Requester(GATTRequester):
+    """
+    Wrapper to access `on_notification` capability of GATT
+    Set "notification_sink" field to a callable that will handle incoming data
+    """
+
+    def __init__(self, p_object, *args, **kwargs):
+        super(Requester, self).__init__(p_object, *args, **kwargs)
+        self.notification_sink = None
+
+        self._notify_queue = queue.Queue()  # this queue is to minimize time spent in gattlib C code
+        self.notify_thread = Thread(target=self._dispatch_notifications)
+        self.notify_thread.setDaemon(True)
+        self.notify_thread.setName("Notify queue dispatcher")
+        self.notify_thread.start()
+
+    def on_notification(self, handle, data):
+        # log.debug("requester notified, sink: %s", self.notification_sink)
+        self._notify_queue.put((handle, data))
+
+    def on_indication(self, handle, data):
+        log.debug("Indication on handle %s: %s", handle, str2hex(data))
+
+    def _dispatch_notifications(self):
+        while True:
+            handle, data = self._notify_queue.get()
+            data = data[3:]  # for some reason, there are extra bytes
+            if self.notification_sink:
+                try:
+                    self.notification_sink(handle, data)
+                except BaseException:
+                    log.warning("Data was: %s", str2hex(data))
+                    log.warning("Failed to dispatch notification: %s", traceback.format_exc())
+            else:
+                log.warning("Dropped notification %s: %s", handle, str2hex(data))
+
+
+class GattLibConnection(Connection):
+    """
+    Main transport class, uses real Bluetooth LE connection.
+    Loops with timeout of 1 seconds to find device named "Lego MOVE Hub"
+
+    :type requester: Requester
+    """
+
+    def __init__(self, bt_iface_name='hci0'):
+        super(GattLibConnection, self).__init__()
+        self.requester = None
+        self._iface = bt_iface_name
+
+    def connect(self, hub_mac=None, hub_name=None):
+        service = DiscoveryService(self._iface)
+
+        while not self.requester:
+            log.info("Discovering devices using %s...", self._iface)
+            devices = service.discover(1)
+            log.debug("Devices: %s", devices)
+
+            for address, name in devices.items():
+                if self._is_device_matched(address, name, hub_mac, hub_name):
+                    self.requester = Requester(address, True, self._iface)
+                    break
+
+            if self.requester:
+                break
+
+        return self
+
+    def set_notify_handler(self, handler):
+        if self.requester:
+            log.debug("Setting notification handler: %s", handler)
+            self.requester.notification_sink = handler
+        else:
+            raise RuntimeError("No requester available")
+
+    def write(self, handle, data):
+        log.debug("Writing to %s: %s", handle, str2hex(data))
+        return self.requester.write_by_handle(handle, data)
+
+    def is_alive(self):
+        return self.requester.notify_thread.isAlive()

pylgbst/comms/cpygatt.py

@@ -0,0 +1,63 @@
+import logging
+
+import pygatt
+
+from pylgbst.comms import Connection, MOVE_HUB_HW_UUID_CHAR
+from pylgbst.utilities import str2hex
+
+log = logging.getLogger('comms-pygatt')
+
+
+class GattoolConnection(Connection):
+    """
+    Used for connecting to
+
+    :type _conn_hnd: pygatt.backends.bgapi.device.BGAPIBLEDevice
+    """
+
+    def __init__(self, controller='hci0'):
+        Connection.__init__(self)
+        self.backend = lambda: pygatt.GATTToolBackend(hci_device=controller)
+        self._conn_hnd = 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()  # 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 self._is_device_matched(address, name, hub_mac, hub_name):
+                    self._conn_hnd = adapter.connect(address)
+                    break
+
+            if self._conn_hnd:
+                break
+
+        return self
+
+    def disconnect(self):
+        self._conn_hnd.disconnect()
+
+    def write(self, handle, data):
+        log.debug("Writing to handle %s: %s", handle, str2hex(data))
+        return self._conn_hnd.char_write_handle(handle, bytearray(data))
+
+    def set_notify_handler(self, handler):
+        self._conn_hnd.subscribe(MOVE_HUB_HW_UUID_CHAR, handler)
+
+    def is_alive(self):
+        return True
+
+
+class BlueGigaConnection(GattoolConnection):
+    def __init__(self):
+        super(BlueGigaConnection, self).__init__()
+        self.backend = lambda: pygatt.BGAPIBackend()

pylgbst/constants.py

@@ -1,144 +0,0 @@
-# GENERAL
-LEGO_MOVE_HUB = "LEGO Move Hub"
-DEVICE_NAME = 0x07
-MOVE_HUB_HARDWARE_HANDLE = 0x0E
-MOVE_HUB_HARDWARE_UUID = '00001624-1212-efde-1623-785feabcd123'
-
-ENABLE_NOTIFICATIONS_HANDLE = 0x000f
-ENABLE_NOTIFICATIONS_VALUE = b'\x01\x00'
-
-PACKET_VER = 0x01
-
-# PORTS
-PORT_C = 0x01
-PORT_D = 0x02
-PORT_LED = 0x32
-PORT_A = 0x37
-PORT_B = 0x38
-PORT_AB = 0x39
-PORT_TILT_SENSOR = 0x3A
-PORT_SOMETHING1 = 0x3B
-PORT_BATTERY = 0x3C  # subscribing on this port showed correlation with power voltage
-
-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_SOMETHING1: "UNK1",
-    PORT_BATTERY: "BATTERY",
-}
-
-# 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_UNKNOWN1 = 0x15  # one of them is button? onboard temperature? maybe another kind of voltage, they have same params
-DEV_BATTERY = 0x14
-DEV_DCS = 0x25
-DEV_IMOTOR = 0x26
-DEV_MOTOR = 0x27
-DEV_TILT_SENSOR = 0x28
-DEV_LED = 0x17
-
-DEVICE_TYPES = {
-    DEV_DCS: "DISTANCE_COLOR_SENSOR",
-    DEV_IMOTOR: "IMOTOR",
-    DEV_MOTOR: "MOTOR",
-    DEV_TILT_SENSOR: "TILT_SENSOR",
-    DEV_LED: "LED",
-    DEV_UNKNOWN1: "UNKNOWN #1",
-    DEV_BATTERY: "BATTERY",
-}
-
-# NOTIFICATIONS
-STATUS_STARTED = 0x01
-STATUS_CONFLICT = 0x05
-STATUS_FINISHED = 0x0a
-
-# TILT SENSOR
-TILT_MODE_2AXIS_FULL = 0x00
-TILT_MODE_2AXIS_SIMPLE = 0x01
-TILT_MODE_BASIC = 0x02
-TILT_MODE_BUMP = 0x03
-TILT_MODE_FULL = 0x04
-
-TILT_HORIZONTAL = 0x00
-TILT_UP = 0x01
-TILT_DOWN = 0x02
-TILT_RIGHT = 0x03
-TILT_LEFT = 0x04
-TILT_FRONT = 0x05
-TILT_SOME1 = 0x07
-TILT_SOME2 = 0x09
-
-TILT_STATES = {
-    TILT_HORIZONTAL: "HORIZONTAL",
-    TILT_UP: "UP",
-    TILT_DOWN: "DOWN",
-    TILT_RIGHT: "RIGHT",
-    TILT_LEFT: "LEFT",
-    TILT_FRONT: "FRONT",
-    TILT_SOME1: "LEFT1",
-    TILT_SOME2: "SOME2",
-}
-
-# COLOR & DISTANCE SENSOR
-CDS_MODE_COLOR_ONLY = 0x00
-CDS_MODE_DISTANCE_INCHES = 0x01
-CDS_MODE_COUNT_2INCH = 0x02
-CDS_MODE_DISTANCE_HOW_CLOSE = 0x03
-CDS_MODE_DISTANCE_SUBINCH_HOW_CLOSE = 0x04
-CDS_MODE_OFF1 = 0x05
-CDS_MODE_STREAM_3_VALUES = 0x06
-CDS_MODE_OFF2 = 0x07
-CDS_MODE_COLOR_DISTANCE_FLOAT = 0x08
-CDS_MODE_LUMINOSITY = 0x09
-CDS_MODE_SOME_20BYTES = 0x0a
-
-# 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"
-}
-
-# MOTORS
-MOTOR_MODE_SOMETHING1 = 0x00
-MOTOR_MODE_SPEED = 0x01
-MOTOR_MODE_ANGLE = 0x02

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/peripherals.py

@@ -1,68 +1,118 @@
 import logging
-import time
+import math
+import traceback
 from struct import pack, unpack
 from threading import Thread
 
-from six.moves import queue
-
-from pylgbst import get_byte, str2hex
-from pylgbst.constants import *
+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: 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
+        :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
-        self._incoming_port_data = queue.Queue()
+        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)
         thr.setName("Port data queue: %s" % self)
         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", PACKET_VER) + pack("<B", msg_type) + pack("<B", self.port)
-        cmd += params
-        self.parent.connection.write(MOVE_HUB_HARDWARE_HANDLE, pack("<B", len(cmd) + 1) + 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("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("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 is_working(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):
-        self._port_subscription_mode = mode
-        self._port_subscribe(self._port_subscription_mode, granularity, True)
+    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)
 
@@ -70,67 +120,145 @@ class Peripheral(object):
         if callback in self._subscribers:
             self._subscribers.remove(callback)
 
-        if not self._port_subscription_mode:
-            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._port_subscribe(self._port_subscription_mode, 0, False)
-            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):
-        self._incoming_port_data.put(data)
+    def queue_port_data(self, msg):
+        try:
+            self._incoming_port_data.put_nowait(msg)
+        except queue.Full:
+            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("Failed to handle port data by %s: %s", self, str2hex(data))
+                log.warning("%s", traceback.format_exc())
+                log.warning("Failed to handle port data by %s: %r", self, msg)
+
+    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
+        return descr
 
 
-class LED(Peripheral):
-    SOMETHING = b'\x51\x00'
+class LEDRGB(Peripheral):
+    MODE_INDEX = 0x00
+    MODE_RGB = 0x01
 
-    def set_color(self, color, do_notify=True):
-        if color == COLOR_NONE:
-            color = COLOR_BLACK
+    def __init__(self, parent, port):
+        super(LEDRGB, self).__init__(parent, port)
 
-        if color not in COLORS:
-            raise ValueError("Color %s is not in list of available colors" % color)
+    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
 
-        cmd = pack("<?", do_notify) + self.SOMETHING + pack("<B", color)
-        self.started()
-        self._write_to_hub(MSG_SET_PORT_VAL, cmd)
+            if color not in COLORS:
+                raise ValueError("Color %s is not in list of available colors" % color)
 
-    def finished(self):
-        super(LED, self).finished()
-        log.debug("LED has changed color")
-        self._notify_subscribers()
+            self.set_port_mode(self.MODE_INDEX)
+            payload = pack("<B", self.MODE_INDEX) + pack("<B", color)
 
-    def subscribe(self, callback, mode=None, granularity=None):
-        self._subscribers.add(callback)
+        msg = MsgPortOutput(self.port, MsgPortOutput.WRITE_DIRECT_MODE_DATA, payload)
+        self._send_output(msg)
 
-    def unsubscribe(self, callback=None):
-        self._subscribers.add(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
-    MOVEMENT_TYPE = b'\x11'
-    TIMED_SINGLE = b'\x09'
-    TIMED_GROUP = b'\x0A'
-    ANGLED_SINGLE = b'\x0B'
-    ANGLED_GROUP = b'\x0C'
+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
+
+    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
@@ -139,174 +267,378 @@ class EncodedMotor(Peripheral):
             log.warning("Speed cannot be more than 1")
             relative = 1
 
-        absolute = round(relative * 100)
-        if absolute < 0:
-            absolute += 255
+        absolute = math.ceil(relative * 100)  # scale of 100 is proven by experiments
         return int(absolute)
 
-    def _wrap_and_write(self, command, speed_primary, speed_secondary):
-        # set for port
-        command = self.MOVEMENT_TYPE + command
+    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)
 
-        command += pack("<B", self._speed_abs(speed_primary))
-        if self.port == PORT_AB:
-            command += pack("<B", self._speed_abs(speed_secondary))
+    def _send_cmd(self, subcmd, params):
+        if self.virtual_ports:
+            subcmd += 1  # de-facto rule
 
-        command += self.TRAILER
+        msg = MsgPortOutput(self.port, subcmd, params)
+        self._send_output(msg)
 
-        self._write_to_hub(MSG_SET_PORT_VAL, command)
+    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
 
-    def timed(self, seconds, speed_primary=1, speed_secondary=None, async=False):
+        if self.virtual_ports:
+            cmd = self.SUBCMD_START_POWER_GROUPED - 1  # because _send_cmd will do +1
+        else:
+            cmd = self.SUBCMD_START_POWER
+
+        params = b""
+        params += pack("<b", self._speed_abs(power_primary))
+        if self.virtual_ports:
+            params += pack("<b", self._speed_abs(power_secondary))
+
+        self._send_cmd(cmd, params)
+
+    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
 
-        # movement type
-        command = self.TIMED_GROUP if self.port == PORT_AB else self.TIMED_SINGLE
-        # time
-        command += 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(command, 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
 
-        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
 
-        # movement type
-        command = self.ANGLED_GROUP if self.port == PORT_AB else self.ANGLED_SINGLE
-        # angle
-        command += 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(command, 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 __wait_sync(self, async):
-        if not async:
-            log.debug("Waiting for sync command work to finish...")
-            while self.is_working():
-                time.sleep(0.5)
-            log.debug("Command has finished.")
+        self._send_cmd(self.SUBCMD_START_SPEED_FOR_DEGREES, params)
 
-    def handle_port_data(self, data):
-        if self._port_subscription_mode == MOTOR_MODE_ANGLE:
-            rotation = unpack("<l", data[4:8])[0]
-            self._notify_subscribers(rotation)
-        elif self._port_subscription_mode == MOTOR_MODE_SOMETHING1:
-            # TODO: understand what it means
-            rotation = unpack("<B", data[4])[0]
-            self._notify_subscribers(rotation)
-        elif self._port_subscription_mode == MOTOR_MODE_SPEED:
-            rotation = unpack("<b", data[4])[0]
-            self._notify_subscribers(rotation)
+    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
+
+        params = b""
+        params += pack("<i", degrees_primary)
+        if self.virtual_ports:
+            params += pack("<i", degrees_secondary)
+
+        params += pack("<b", self._speed_abs(speed))
+
+        params += pack("<B", int(100 * max_power))
+        params += pack("<B", end_state)
+        params += pack("<B", use_profile)
+
+        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=MOTOR_MODE_ANGLE, granularity=1):
+    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):
-    def subscribe(self, callback, mode=TILT_MODE_BASIC, granularity=1):
+    MODE_2AXIS_ANGLE = 0x00
+    MODE_2AXIS_SIMPLE = 0x01
+    MODE_3AXIS_SIMPLE = 0x02
+    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
+    TRI_DOWN = 0x02
+    TRI_LEFT = 0x03
+    TRI_RIGHT = 0x04
+    TRI_FRONT = 0x05
+
+    DUO_HORIZ = 0x00
+    DUO_DOWN = 0x03
+    DUO_LEFT = 0x05
+    DUO_RIGHT = 0x07
+    DUO_UP = 0x09
+
+    DUO_STATES = {
+        DUO_HORIZ: "HORIZONTAL",
+        DUO_DOWN: "DOWN",
+        DUO_LEFT: "LEFT",
+        DUO_RIGHT: "RIGHT",
+        DUO_UP: "UP",
+    }
+
+    TRI_STATES = {
+        TRI_BACK: "BACK",
+        TRI_UP: "UP",
+        TRI_DOWN: "DOWN",
+        TRI_LEFT: "LEFT",
+        TRI_RIGHT: "RIGHT",
+        TRI_FRONT: "FRONT",
+    }
+
+    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 == TILT_MODE_BASIC:
-            state = get_byte(data, 4)
-            self._notify_subscribers(state)
-        elif self._port_subscription_mode == TILT_MODE_2AXIS_SIMPLE:
-            # TODO: figure out right interpreting of this
-            state = get_byte(data, 4)
-            self._notify_subscribers(state)
-        elif self._port_subscription_mode == TILT_MODE_BUMP:
-            bump_count = get_byte(data, 4)
-            self._notify_subscribers(bump_count)
-        elif self._port_subscription_mode == TILT_MODE_2AXIS_FULL:
-            roll = self._byte2deg(get_byte(data, 4))
-            pitch = self._byte2deg(get_byte(data, 5))
-            self._notify_subscribers(roll, pitch)
-        elif self._port_subscription_mode == TILT_MODE_FULL:
-            roll = self._byte2deg(get_byte(data, 4))
-            pitch = self._byte2deg(get_byte(data, 5))
-            yaw = self._byte2deg(get_byte(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):
+class VisionSensor(Peripheral):
+    COLOR_INDEX = 0x00
+    DISTANCE_INCHES = 0x01
+    COUNT_2INCH = 0x02
+    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=CDS_MODE_COLOR_DISTANCE_FLOAT, granularity=1):
-        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 == CDS_MODE_COLOR_DISTANCE_FLOAT:
-            color = get_byte(data, 4)
-            distance = get_byte(data, 5)
-            partial = get_byte(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 == CDS_MODE_COLOR_ONLY:
-            color = get_byte(data, 4)
-            self._notify_subscribers(color)
-        elif self._port_subscription_mode == CDS_MODE_DISTANCE_INCHES:
-            distance = get_byte(data, 4)
-            self._notify_subscribers(distance)
-        elif self._port_subscription_mode == CDS_MODE_DISTANCE_HOW_CLOSE:
-            distance = get_byte(data, 4)
-            self._notify_subscribers(distance)
-        elif self._port_subscription_mode == CDS_MODE_DISTANCE_SUBINCH_HOW_CLOSE:
-            distance = get_byte(data, 4)
-            self._notify_subscribers(distance)
-        elif self._port_subscription_mode == CDS_MODE_OFF1 or self._port_subscription_mode == CDS_MODE_OFF2:
-            log.info("Turned off led on %s", self)
-        elif self._port_subscription_mode == CDS_MODE_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 == CDS_MODE_STREAM_3_VALUES:
-            # TODO: understand better meaning of these 3 values
-            val1 = unpack("<H", data[4:6])[0]
-            val2 = unpack("<H", data[6:8])[0]
-            val3 = unpack("<H", data[8:10])[0]
-            self._notify_subscribers(val1, val2, val3)
-        elif self._port_subscription_mode == CDS_MODE_LUMINOSITY:
-            luminosity = unpack("<H", data[4:6])[0] / 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 Battery(Peripheral):
+class Voltage(Peripheral):
+    # sensor says there are "L" and "S" values, but what are they?
+    VOLTAGE_L = 0x00
+    VOLTAGE_S = 0x01
+
     def __init__(self, parent, port):
-        super(Battery, self).__init__(parent, port)
-        self.last_value = None
+        super(Voltage, self).__init__(parent, port)
 
-    def subscribe(self, callback, mode=0, granularity=1):
-        super(Battery, self).subscribe(callback, mode, granularity)
+    def _decode_port_data(self, msg):
+        data = msg.payload
+        val = ushort(data, 0)
+        volts = 9600.0 * val / 3893.0 / 1000.0
+        return (volts,)
 
-    # we know only voltage subscription from it. is it really battery or just onboard voltage?
-    # device has turned off on 1b0e000600453ba800 - 168d
-    # moderate 9v ~= 3840
-    # good 7.5v ~= 3892
-    # liion 5v ~= 2100
-    def handle_port_data(self, data):
-        self.last_value = unpack("<h", data[4:6])[0]
-        self._notify_subscribers(self.last_value)
+
+class Current(Peripheral):
+    CURRENT_L = 0x00
+    CURRENT_S = 0x01
+
+    def __init__(self, parent, port):
+        super(Current, self).__init__(parent, port)
+
+    def _decode_port_data(self, msg):
+        val = ushort(msg.payload, 0)
+        milliampers = 2444 * val / 4095.0
+        return (milliampers,)
 
 
 class Button(Peripheral):
@@ -315,11 +647,12 @@ class Button(Peripheral):
     """
 
     def __init__(self, parent):
-        super(Button, self).__init__(parent, 0)
+        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):
-        cmd = pack("<B", PACKET_VER) + pack("<B", MSG_DEVICE_INFO) + b'\x02\x02'
-        self.parent.connection.write(MOVE_HUB_HARDWARE_HANDLE, pack("<B", len(cmd) + 1) + cmd)
+        self.hub.send(MsgHubProperties(MsgHubProperties.BUTTON, MsgHubProperties.UPD_ENABLE))
+
         if callback:
             self._subscribers.add(callback)
 
@@ -328,8 +661,11 @@ class Button(Peripheral):
             self._subscribers.remove(callback)
 
         if not self._subscribers:
-            cmd = pack("<B", PACKET_VER) + pack("<B", MSG_DEVICE_INFO) + b'\x02\x03'
-            self.parent.connection.write(MOVE_HUB_HARDWARE_HANDLE, pack("<B", len(cmd) + 1) + cmd)
+            self.hub.send(MsgHubProperties(MsgHubProperties.BUTTON, MsgHubProperties.UPD_DISABLE))
 
-    def handle_port_data(self, data):
-        self._notify_subscribers(bool(unpack("<B", data[5:6])[0]))
+    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

@@ -0,0 +1,45 @@
+"""
+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:
+    import queue as queue
+
+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 check_unpack(seq, index, "<B", 1)
+
+
+def ushort(seq, index):
+    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
+    # 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,10 +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.2',
-      author='Andrey Pokhilko',
-      author_email='apc4@ya.ru',
-      packages=['pylgbst'],
-      requires=['six', 'gattlib']
-      )
+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.py

@@ -1,162 +0,0 @@
-import unittest
-
-from pylgbst import *
-
-HANDLE = MOVE_HUB_HARDWARE_HANDLE
-
-logging.basicConfig(level=logging.DEBUG)
-
-log = logging.getLogger('test')
-
-
-class ConnectionMock(Connection):
-    """
-    For unit testing purposes
-    """
-
-    def __init__(self):
-        super(ConnectionMock, self).__init__()
-        self.writes = []
-        self.notifications = []
-        self.notification_handler = None
-        self.running = True
-        self.finished = False
-
-    def set_notify_handler(self, handler):
-        self.notification_handler = handler
-        thr = Thread(target=self.notifier)
-        thr.setDaemon(True)
-        thr.start()
-
-    def notifier(self):
-        while self.running:
-            if self.notification_handler:
-                while self.notifications:
-                    handle, data = self.notifications.pop(0)
-                    self.notification_handler(handle, unhexlify(data.replace(' ', '')))
-            time.sleep(0.1)
-
-        self.finished = True
-
-    def write(self, handle, data):
-        log.debug("Writing to %s: %s", handle, str2hex(data))
-        self.writes.append((handle, str2hex(data)))
-
-
-class HubMock(MoveHub):
-    def __init__(self, connection=None):
-        super(HubMock, self).__init__(connection if connection else ConnectionMock())
-
-    def _wait_for_devices(self):
-        pass
-
-
-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")
-                break
-
-    def test_led(self):
-        hub = HubMock()
-        led = LED(hub, PORT_LED)
-        led.set_color(COLOR_RED)
-        self.assertEqual("0801813201510009", hub.connection.writes[0][1])
-
-    def test_tilt_sensor(self):
-        hub = HubMock()
-        hub.connection.notifications.append((HANDLE, '1b0e00 0f00 04 3a 0128000000000100000001'))
-        time.sleep(1)
-
-        def callback(param1, param2=None, param3=None):
-            if param2 is None:
-                log.debug("Tilt: %s", TILT_STATES[param1])
-            else:
-                log.debug("Tilt: %s %s %s", param1, param2, param3)
-
-        hub.tilt_sensor.subscribe(callback)
-        hub.connection.notifications.append((HANDLE, "1b0e000500453a05"))
-        hub.connection.notifications.append((HANDLE, "1b0e000a00473a010100000001"))
-        time.sleep(1)
-        hub.tilt_sensor.subscribe(callback, TILT_MODE_2AXIS_SIMPLE)
-
-        hub.connection.notifications.append((HANDLE, "1b0e000500453a09"))
-        time.sleep(1)
-
-        hub.tilt_sensor.subscribe(callback, TILT_MODE_2AXIS_FULL)
-        hub.connection.notifications.append((HANDLE, "1b0e000600453a04fe"))
-        time.sleep(1)
-
-        self._wait_notifications_handled(hub)
-        hub.tilt_sensor.unsubscribe(callback)
-        # TODO: assert
-
-    def test_motor(self):
-        conn = ConnectionMock()
-        conn.notifications.append((14, '1b0e00 0900 04 39 0227003738'))
-        hub = HubMock(conn)
-        time.sleep(0.1)
-
-        conn.notifications.append((14, '1b0e00050082390a'))
-        hub.motor_AB.timed(1.5)
-        self.assertEqual("0d018139110adc056464647f03", conn.writes[0][1])
-
-        conn.notifications.append((14, '1b0e00050082390a'))
-        hub.motor_AB.angled(90)
-        self.assertEqual("0f018139110c5a0000006464647f03", conn.writes[1][1])
-
-    def test_capabilities(self):
-        conn = ConnectionMock()
-        conn.notifications.append((14, '1b0e00 0f00 04 01 0125000000001000000010'))
-        conn.notifications.append((14, '1b0e00 0f00 04 02 0126000000001000000010'))
-        conn.notifications.append((14, '1b0e00 0f00 04 37 0127000100000001000000'))
-        conn.notifications.append((14, '1b0e00 0f00 04 38 0127000100000001000000'))
-        conn.notifications.append((14, '1b0e00 0900 04 39 0227003738'))
-        conn.notifications.append((14, '1b0e00 0f00 04 32 0117000100000001000000'))
-        conn.notifications.append((14, '1b0e00 0f00 04 3a 0128000000000100000001'))
-        conn.notifications.append((14, '1b0e00 0f00 04 3b 0115000200000002000000'))
-        conn.notifications.append((14, '1b0e00 0f00 04 3c 0114000200000002000000'))
-        conn.notifications.append((14, '1b0e00 0f00 8202 01'))
-        conn.notifications.append((14, '1b0e00 0f00 8202 0a'))
-
-        hub = MoveHub(conn)
-        # demo_all(hub)
-        self._wait_notifications_handled(hub)
-
-    def test_color_sensor(self):
-        #
-        hub = HubMock()
-        hub.connection.notifications.append((HANDLE, '1b0e000f0004010125000000001000000010'))
-        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)
-
-        hub.color_distance_sensor.subscribe(callback)
-
-        hub.connection.notifications.append((HANDLE, "1b0e0008004501ff0aff00"))
-        time.sleep(1)
-        # TODO: assert
-        self._wait_notifications_handled(hub)
-        hub.color_distance_sensor.unsubscribe(callback)
-
-    def test_button(self):
-        hub = HubMock()
-        time.sleep(1)
-
-        def callback(pressed):
-            log.info("Pressed: %s", pressed)
-
-        hub.button.subscribe(callback)
-
-        hub.connection.notifications.append((HANDLE, "1b0e00060001020601"))
-        hub.connection.notifications.append((HANDLE, "1b0e00060001020600"))
-        time.sleep(1)
-        # TODO: assert
-        self._wait_notifications_handled(hub)
-        hub.button.unsubscribe(callback)

tests/__init__.py

@@ -0,0 +1,87 @@
+import sys
+import time
+from binascii import unhexlify
+
+from pylgbst.comms import Connection
+from pylgbst.hub import MoveHub, Hub
+from pylgbst.peripherals import *
+
+logging.basicConfig(level=logging.DEBUG if 'pydevd' in sys.modules else logging.INFO)
+
+log = logging.getLogger('test')
+
+
+class HubMock(Hub):
+    """
+    :type connection: ConnectionMock
+    """
+
+    # noinspection PyUnresolvedReferences
+    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
+
+
+class ConnectionMock(Connection):
+    """
+    For unit testing purposes
+    """
+
+    def __init__(self):
+        super(ConnectionMock, self).__init__()
+        self.writes = []
+        self.notifications = []
+        self.notification_handler = None
+        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
+        self.thr.start()
+
+    def notifier(self):
+        while self.running or self.notifications:
+            if self.notification_handler:
+                while self.notifications:
+                    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

@@ -0,0 +1,50 @@
+import dbus
+import sys
+import unittest
+
+from gatt import DeviceManager
+
+from pylgbst.comms.cgatt import CustomDevice, GattConnection
+from tests import log, str2hex
+
+
+class MockBus(object):
+    def __init__(self, *args, **kwargs):
+        # super(MockBus, self).__init__(*args, **kwargs)
+        pass
+
+    def get_object(self, bus_name, object_path, introspect=True, follow_name_owner_changes=False, **kwargs):
+        return None
+
+
+dbus.SystemBus = lambda: MockBus()
+
+
+class DeviceManagerMock(DeviceManager, object):
+    def update_devices(self):
+        pass
+
+
+class TestGatt(unittest.TestCase):
+    def test_one(self):
+        log.debug("")
+        manager = DeviceManagerMock("hci0")
+        obj = CustomDevice("AA", manager)
+
+        def callback(handle, value):
+            log.debug("%s: %s", type(value), str2hex(value))
+            if sys.version_info[0] == 2:
+                self.assertEquals("0f0004020126000000001000000010", str2hex(value))
+
+        obj.set_notific_handler(callback)
+        arr = "dbus.Array([dbus.Byte(15), dbus.Byte(0), dbus.Byte(4), dbus.Byte(2), dbus.Byte(1), dbus.Byte(38), " \
+              "dbus.Byte(0), dbus.Byte(0), dbus.Byte(0), dbus.Byte(0), dbus.Byte(16), dbus.Byte(0), dbus.Byte(0), " \
+              "dbus.Byte(0), dbus.Byte(16)], signature=dbus.Signature('y'), variant_level=1)"
+        obj.characteristic_value_updated(None, arr if sys.version_info[0] == 2 else bytes(arr, 'ascii'))
+
+    def test_conn(self):
+        try:
+            obj = GattConnection()
+            obj.connect()
+        except AttributeError:
+            pass

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_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

@@ -0,0 +1,69 @@
+import unittest
+
+import pygatt
+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
+
+
+class SerialMock(serial.Serial):
+    def write(self, data):
+        self.is_open = True
+        log.debug("Write data to serial: %s", data)
+        return len(data)
+
+    def flush(self, *args, **kwargs):
+        pass
+
+    def close(self):
+        pass
+
+    def read(self, size=1):
+        return bytes()
+
+
+class BGAPIBLEDeviceMock(BGAPIBLEDevice):
+    def subscribe(self, uuid, callback=None, indication=False):
+        log.debug("Mock subscribing")
+
+    def char_write_handle(self, char_handle, value, wait_for_response=False):
+        log.debug("Mock write: %s", value)
+
+
+class BlueGigaBackendMock(pygatt.BGAPIBackend):
+    def _open_serial_port(self, max_connection_attempts=MAX_CONNECTION_ATTEMPTS):
+        log.debug("Mock open serial port")
+        self._ser = SerialMock()
+
+    def expect(self, expected, *args, **kargs):
+        log.debug("Mock expect")
+        data = {
+            "packet_type": 0x04,
+            "sender": "abcdef".encode('ascii'),
+            "data": [1, 2, 3],
+            "rssi": 1
+        }
+        self._ble_evt_gap_scan_response(data)
+
+    def connect(self, address, timeout=5, address_type=BLEAddressType.public, interval_min=60, interval_max=76,
+                supervision_timeout=100, latency=0):
+        log.debug("Mock connect")
+        device = BGAPIBLEDeviceMock("address", 0, self)
+        return device
+
+    def _detect_device_port(self):
+        return USBSerialDeviceInfo().port_name
+
+
+class BlueGigaTests(unittest.TestCase):
+    def test_1(self):
+        obj = GattoolConnection()
+        obj.backend = BlueGigaBackendMock
+        obj.connect(u'66:65:64:63:62:61')
+        obj.write(0, "test".encode('ascii'))
+        obj.set_notify_handler(lambda x: None)