diff --git a/.gitignore b/.gitignore index 427d53a..ef136e8 100644 --- a/.gitignore +++ b/.gitignore @@ -3,3 +3,4 @@ *.pyc build *.avi +test_real.py \ No newline at end of file diff --git a/.travis.yml b/.travis.yml index eeae57c..b505643 100644 --- a/.travis.yml +++ b/.travis.yml @@ -5,33 +5,33 @@ virtualenv: matrix: include: - - os: linux - python: 2.7 - - os: linux - python: 3.4 + - os: linux + python: 2.7 + - os: linux + python: 3.4 addons: apt: packages: - - libboost-python-dev - - libboost-thread-dev - - libbluetooth-dev + - libboost-python-dev + - libboost-thread-dev + - libbluetooth-dev - - libglib2.0-dev - - libdbus-1-dev - - libdbus-glib-1-dev - - libgirepository-1.0-1 + - libglib2.0-dev + - libdbus-1-dev + - libdbus-glib-1-dev + - libgirepository-1.0-1 - - python-dbus - - python-gi - - python3-dbus - - python3-gi + - python-dbus + - python-gi + - python3-dbus + - python3-gi install: -- pip install codecov nose-exclude gattlib pygatt gatt pexpect bluepy + - pip install codecov nose-exclude gattlib pygatt gatt pexpect bluepy -script: coverage run --source=. `which nosetests` tests --nocapture --exclude-dir=examples +script: coverage run --source=. -m nose tests -v --exclude-dir=examples after_success: -- coverage report -m -- codecov + - coverage report -m + - codecov diff --git a/README.md b/README.md index ff8644c..456bca7 100644 --- a/README.md +++ b/README.md @@ -8,7 +8,7 @@ Best way to start is to look into [`demo.py`](examples/demo.py) file, and run it If you have Vernie assembled, you might run scripts from [`examples/vernie`](examples/vernie) directory. -Demonstrational videos: +## Demonstrational Videos [![Vernie Programmed](http://img.youtube.com/vi/oqsmgZlVE8I/0.jpg)](http://www.youtube.com/watch?v=oqsmgZlVE8I) [![Laser Engraver](http://img.youtube.com/vi/ZbKmqVBBMhM/0.jpg)](https://youtu.be/ZbKmqVBBMhM) @@ -18,16 +18,17 @@ Demonstrational videos: ## Features -- auto-detect and connect to Move Hub device -- auto-detects peripheral devices connected to Hub -- constant, angled and timed movement for motors, rotation sensor subscription -- color & distance sensor: several modes to measure distance, color and luminosity -- tilt sensor subscription: 2 axis, 3 axis, bump detect modes -- LED color change -- push button status subscription -- battery voltage subscription available +- auto-detect and connect to [Move Hub](docs/MoveHub.md) device +- auto-detects [peripheral devices](docs/Peripherals.md) connected to Hub +- constant, angled and timed movement for [motors](docs/Motor.md), rotation sensor subscription +- [vision sensor](docs/VisionSensor.md): several modes to measure distance, color and luminosity +- [tilt sensor](docs/TiltSensor.md) subscription: 2 axis, 3 axis, bump detect modes +- [RGB LED](docs/LED.md) color change +- [push button](docs/MoveHub.md#push-button) status subscription +- [battery voltage and current](docs/VoltageCurrent.md) subscription available - permanent Bluetooth connection server for faster debugging + ## Usage _Please note that this library requires one of Bluetooth backend libraries to be installed, please read section [here](#bluetooth-backend-prerequisites) for details._ @@ -40,217 +41,17 @@ pip install https://github.com/undera/pylgbst/archive/1.0.tar.gz Then instantiate MoveHub object and start invoking its methods. Following is example to just print peripherals detected on Hub: ```python -from pylgbst.movehub import MoveHub +from pylgbst.hub import MoveHub hub = MoveHub() -for device in hub.devices: +for device in hub.peripherals: print(device) ``` -### Controlling Motors - -MoveHub provides motors via following fields: -- `motor_A` - port A -- `motor_B` - port B -- `motor_AB` - motor group of A+B manipulated together -- `motor_external` - external motor attached to port C or D - -Methods to activate motors are: -- `constant(speed_primary, speed_secondary)` - enables motor with specified speed forever -- `timed(time, speed_primary, speed_secondary)` - enables motor with specified speed for `time` seconds, float values accepted -- `angled(angle, speed_primary, speed_secondary)` - makes motor to rotate to specified angle, `angle` value is integer degrees, can be negative and can be more than 360 for several rounds -- `stop()` - stops motor at once, it is equivalent for `constant(0)` - -Parameter `speed_secondary` is used when it is motor group of `motor_AB` running together. By default, `speed_secondary` equals `speed_primary`. Speed values range is `-1.0` to `1.0`, float values. _Note: In group angled mode, total rotation angle is distributed across 2 motors according to motor speeds ratio._ - -All these methods are synchronous by default, means method does not return untill it gets confirmation from Hub that command has completed. You can pass `async=True` parameter to any of methods to switch into asynchronous, which means command will return immediately, without waiting for rotation to complete. Be careful with asynchronous calls, as they make Hub to stop reporting synchronizing statuses. - -An example: -```python -from pylgbst.movehub import MoveHub -import time - -hub = MoveHub() - -hub.motor_A.timed(0.5, 0.8) -hub.motor_A.timed(0.5, -0.8) - -hub.motor_B.angled(90, 0.8) -hub.motor_B.angled(-90, 0.8) - -hub.motor_AB.timed(1.5, 0.8, -0.8) -hub.motor_AB.angled(90, 0.8, -0.8) - -hub.motor_external.constant(0.2) -time.sleep(2) -hub.motor_external.stop() -``` - - -### Motor Rotation Sensors - -Any motor allows to subscribe to its rotation sensor. Two sensor modes are available: rotation angle (`EncodedMotor.SENSOR_ANGLE`) and rotation speed (`EncodedMotor.SENSOR_SPEED`). Example: - -```python -from pylgbst.movehub import MoveHub, EncodedMotor -import time - -def callback(angle): - print("Angle: %s" % angle) - -hub = MoveHub() - -hub.motor_A.subscribe(callback, mode=EncodedMotor.SENSOR_ANGLE) -time.sleep(60) # rotate motor A -hub.motor_A.unsubscribe(callback) -``` - -### Tilt Sensor - -MoveHub's internal tilt sensor is available through `tilt_sensor` field. There are several modes to subscribe to sensor, providing 2-axis, 3-axis and bump detect data. - -An example: - -```python -from pylgbst.movehub import MoveHub, TiltSensor -import time - -def callback(pitch, roll, yaw): - print("Pitch: %s / Roll: %s / Yaw: %s" % (pitch, roll, yaw)) - -hub = MoveHub() - -hub.tilt_sensor.subscribe(callback, mode=TiltSensor.MODE_3AXIS_FULL) -time.sleep(60) # turn MoveHub block in different ways -hub.tilt_sensor.unsubscribe(callback) -``` - -`TiltSensor` sensor mode constants: -- `MODE_2AXIS_SIMPLE` - use `callback(state)` for 2-axis simple state detect -- `MODE_2AXIS_FULL` - use `callback(roll, pitch)` for 2-axis roll&pitch degree values -- `MODE_3AXIS_SIMPLE` - use `callback(state)` for 3-axis simple state detect -- `MODE_3AXIS_FULL` - use `callback(roll, pitch)` for 2-axis roll&pitch degree values -- `MODE_BUMP_COUNT` - use `callback(count)` to detect bumps - -There are tilt sensor constants for "simple" states, for 2-axis mode their names are also available through `TiltSensor.DUO_STATES`: -- `DUO_HORIZ` - "HORIZONTAL" -- `DUO_DOWN` - "DOWN" -- `DUO_LEFT` - "LEFT" -- `DUO_RIGHT` - "RIGHT" -- `DUO_UP` - "UP" - -For 3-axis simple mode map name is `TiltSensor.TRI_STATES` with values: -- `TRI_BACK` - "BACK" -- `TRI_UP` - "UP" -- `TRI_DOWN` - "DOWN" -- `TRI_LEFT` - "LEFT" -- `TRI_RIGHT` - "RIGHT" -- `TRI_FRONT` - "FRONT" - - -### Color & Distance Sensor - -Field named `color_distance_sensor` holds instance of `ColorDistanceSensor`, if one is attached to MoveHub. Sensor has number of different modes to subscribe. - -Colors that are detected are part of `COLORS` map (see [LED](#led) section). Only several colors are possible to detect: `BLACK`, `BLUE`, `CYAN`, `YELLOW`, `RED`, `WHITE`. Sensor does its best to detect best color, but only works when sample is very close to sensor. - -Distance works in range of 0-10 inches, with ability to measure last inch in higher detail. - -Simple example of subscribing to sensor: - -```python -from pylgbst.movehub import MoveHub, ColorDistanceSensor -import time - -def callback(clr, distance): - print("Color: %s / Distance: %s" % (clr, distance)) - -hub = MoveHub() - -hub.color_distance_sensor.subscribe(callback, mode=ColorDistanceSensor.COLOR_DISTANCE_FLOAT) -time.sleep(60) # play with sensor while it waits -hub.color_distance_sensor.unsubscribe(callback) -``` - -Subscription mode constants in class `ColorDistanceSensor` are: -- `COLOR_DISTANCE_FLOAT` - default mode, use `callback(color, distance)` where `distance` is float value in inches -- `COLOR_ONLY` - use `callback(color)` -- `DISTANCE_INCHES` - use `callback(color)` measures distance in integer inches count -- `COUNT_2INCH` - use `callback(count)` - it counts crossing distance ~2 inches in front of sensor -- `DISTANCE_HOW_CLOSE` - use `callback(value)` - value of 0 to 255 for 30 inches, larger with closer distance -- `DISTANCE_SUBINCH_HOW_CLOSE` - use `callback(value)` - value of 0 to 255 for 1 inch, larger with closer distance -- `LUMINOSITY` - use `callback(luminosity)` where `luminosity` is float value from 0 to 1 -- `OFF1` and `OFF2` - seems to turn sensor LED and notifications off -- `STREAM_3_VALUES` - use `callback(val1, val2, val3)`, sends some values correlating to distance, not well understood at the moment - -Tip: laser pointer pointing to sensor makes it to trigger distance sensor - -### LED - -`MoveHub` class has field `led` to access color LED near push button. To change its color, use `set_color(color)` method. - -You can obtain colors are present as constants `COLOR_*` and also a map of available color-to-name as `COLORS`. There are 12 color values, including `COLOR_BLACK` and `COLOR_NONE` which turn LED off. - -Additionally, you can subscribe to LED color change events, using callback function as shown in example below. - -```python -from pylgbst.movehub import MoveHub, COLORS, COLOR_NONE, COLOR_RED -import time +Each peripheral kind has own methods to do actions and/or get sensor data. See [features](#features) list for individual doc pages. -def callback(clr): - print("Color has changed: %s" % clr) - -hub = MoveHub() -hub.led.subscribe(callback) - -hub.led.set_color(COLOR_RED) -for color in COLORS: - hub.led.set_color(color) - time.sleep(0.5) - -hub.led.set_color(COLOR_NONE) -hub.led.unsubscribe(callback) -``` - -Tip: blinking orange color of LED means battery is low. - -### Push Button - -`MoveHub` class has field `button` to subscribe to button press and release events. - -Note that `Button` class is not real `Peripheral`, as it has no port and not listed in `devices` field of Hub. Still, subscribing to button is done usual way: - -```python -from pylgbst.movehub import MoveHub - -def callback(is_pressed): - print("Btn pressed: %s" % is_pressed) - -hub = MoveHub() -hub.button.subscribe(callback) -``` - -### Power Voltage & Battery - -`MoveHub` class has field `voltage` to subscribe to battery voltage status. Callback accepts single parameter with current value. The range of values is float between `0` and `1.0`. Every time data is received, value is also written into `last_value` field of `Voltage` object. Values less than `0.2` are known as lowest values, when unit turns off. - -```python -from pylgbst.movehub import MoveHub -import time - -def callback(value): - print("Voltage: %s" % value) - -hub = MoveHub() -hub.voltage.subscribe(callback) -time.sleep(1) -print ("Value: " % hub.voltage.last_value) -``` - -## General Notes - -### Bluetooth Backend Prerequisites +## Bluetooth Backend Prerequisites You have following options to install as Bluetooth backend: @@ -278,43 +79,17 @@ There is optional parameter for `MoveHub` class constructor, accepting instance All the functions above have optional arguments to specify adapter name and MoveHub mac address. Please look function source code for details. -If you want to specify name for Bluetooth interface to use on local computer, you can passthat to class or function of getting a connection. Then pass connection object to `MoveHub` constructor. Like this: +If you want to specify name for Bluetooth interface to use on local computer, you can pass that to class or function of getting a connection. Then pass connection object to `MoveHub` constructor. Like this: ```python -from pylgbst.movehub import MoveHub +from pylgbst.hub import MoveHub from pylgbst.comms.cgatt import GattConnection conn = GattConnection("hci1") -conn.connect() # you can pass MoveHub mac address as parameter here, like 'AA:BB:CC:DD:EE:FF' +conn.connect() # you can pass Hub mac address as parameter here, like 'AA:BB:CC:DD:EE:FF' hub = MoveHub(conn) ``` -### Use Disconnect in `finally` - -It is recommended to make sure `disconnect()` method is called on connection object after you have finished your program. This ensures Bluetooth subsystem is cleared and avoids problems for subsequent re-connects of MoveHub. The best way to do that in Python is to use `try ... finally` clause: - -```python -from pylgbst import get_connection_auto -from pylgbst.movehub import MoveHub - -conn=get_connection_auto() # ! don't put this into `try` block -try: - hub = MoveHub(conn) -finally: - conn.disconnect() -``` - -### Devices Detecting -As part of instantiating process, `MoveHub` waits up to 1 minute for all builtin devices to appear, such as motors on ports A and B, tilt sensor, button and battery. This not guarantees that external motor and/or color sensor will be present right after `MoveHub` instantiated. Usually, sleeping for couple of seconds gives it enough time to detect everything. - -### Subscribing to Sensors -Each sensor usually has several different "subscription modes", differing with callback parameters and value semantics. - -There is optional `granularity` parameter for each subscription call, by default it is `1`. This parameter tells Hub when to issue sensor data notification. Value of notification has to change greater or equals to `granularity` to issue notification. This means that specifying `0` will cause it to constantly send notifications, and specifying `5` will cause less frequent notifications, only when values change for more than `5` (inclusive). - -It is possible to subscribe with multiple times for the same sensor. Only one, very last subscribe mode is in effect, with many subscriber callbacks allowed to receive notifications. - -Good practice for any program is to unsubscribe from all sensor subscriptions before ending, especially when used with `DebugServer`. ## Debug Server Running debug server opens permanent BLE connection to Hub and listening on TCP port for communications. This avoids the need to re-start Hub all the time. @@ -331,18 +106,15 @@ Then push green button on MoveHub, so permanent BLE connection will be establish ## Roadmap & TODO +- validate operations with other Hub types (train, PUP etc) +- make connections to detect hub by UUID instead of name - document all API methods -- make sure unit tests cover all important code - make debug server to re-establish BLE connection on loss ## Links - https://github.com/LEGO/lego-ble-wireless-protocol-docs - true docs for LEGO BLE protocol - https://github.com/JorgePe/BOOSTreveng - initial source of protocol knowledge +- https://github.com/nathankellenicki/node-poweredup - JavaScript version of library - https://github.com/spezifisch/sphero-python/blob/master/BB8joyDrive.py - example with another approach to bluetooth libs -Some things around visual programming: -- https://github.com/RealTimeWeb/blockpy -- https://ru.wikipedia.org/wiki/App_Inventor -- https://en.wikipedia.org/wiki/Blockly - diff --git a/docs/GenericHub.md b/docs/GenericHub.md new file mode 100644 index 0000000..a43a787 --- /dev/null +++ b/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. \ No newline at end of file diff --git a/docs/LED.md b/docs/LED.md new file mode 100644 index 0000000..e0078d3 --- /dev/null +++ b/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. \ No newline at end of file diff --git a/docs/Motor.md b/docs/Motor.md new file mode 100644 index 0000000..6403ba6 --- /dev/null +++ b/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) +``` diff --git a/docs/MoveHub.md b/docs/MoveHub.md new file mode 100644 index 0000000..3047d9e --- /dev/null +++ b/docs/MoveHub.md @@ -0,0 +1,36 @@ +# 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) +``` \ No newline at end of file diff --git a/docs/Peripherals.md b/docs/Peripherals.md new file mode 100644 index 0000000..8ed90f9 --- /dev/null +++ b/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. \ No newline at end of file diff --git a/docs/TiltSensor.md b/docs/TiltSensor.md new file mode 100644 index 0000000..8b2692c --- /dev/null +++ b/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" + diff --git a/docs/VisionSensor.md b/docs/VisionSensor.md new file mode 100644 index 0000000..0210299 --- /dev/null +++ b/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 + diff --git a/docs/VoltageCurrent.md b/docs/VoltageCurrent.md new file mode 100644 index 0000000..9f87cad --- /dev/null +++ b/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)) +``` diff --git a/examples/automata/__init__.py b/examples/automata/__init__.py index 4783adc..58194a2 100644 --- a/examples/automata/__init__.py +++ b/examples/automata/__init__.py @@ -2,27 +2,31 @@ import time from collections import Counter -from pylgbst.movehub import MoveHub, COLOR_NONE, COLOR_CYAN, COLOR_BLUE, COLOR_BLACK, COLOR_RED, COLORS -from pylgbst.peripherals import ColorDistanceSensor +from pylgbst.hub import MoveHub, COLOR_NONE, COLOR_BLACK, COLORS, COLOR_CYAN, COLOR_BLUE, COLOR_RED +from pylgbst.peripherals import EncodedMotor class Automata(object): + BASE_SPEED = 0.5 def __init__(self): super(Automata, self).__init__() self.__hub = MoveHub() - self.__hub.color_distance_sensor.subscribe(self.__on_sensor, mode=ColorDistanceSensor.COLOR_ONLY) + self.__hub.vision_sensor.subscribe(self.__on_sensor) self._sensor = [] def __on_sensor(self, color, distance=-1): - if distance < 4: + 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.info("Sensor data: %s", COLORS[color]) + logging.debug("Sensor data: %s", COLORS[color]) def feed_tape(self): - self.__hub.motor_external.angled(120, 0.25) - time.sleep(0.2) + 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 @@ -32,33 +36,47 @@ def get_color(self): clr = cnts.most_common(1)[0][0] if cnts else COLOR_NONE if clr == COLOR_CYAN: clr = COLOR_BLUE - elif clr == COLOR_BLACK: - clr = COLOR_NONE return clr def left(self): - self.__hub.motor_AB.angled(270, 0.25, -0.25) - time.sleep(0.5) + self.__hub.motor_A.angled(270, self.BASE_SPEED, -self.BASE_SPEED, end_state=EncodedMotor.END_STATE_HOLD) + time.sleep(0.1) + self.__hub.motor_A.stop() def right(self): - self.__hub.motor_AB.angled(-270, 0.25, -0.25) - time.sleep(0.5) + self.__hub.motor_B.angled(-320, self.BASE_SPEED, -self.BASE_SPEED, end_state=EncodedMotor.END_STATE_HOLD) + time.sleep(0.1) + self.__hub.motor_B.stop() def forward(self): - self.__hub.motor_AB.angled(830, 0.25) - time.sleep(0.5) + self.__hub.motor_AB.angled(450, self.BASE_SPEED) def backward(self): - self.__hub.motor_AB.angled(830, -0.25) - time.sleep(0.5) + self.__hub.motor_AB.angled(-450, self.BASE_SPEED) if __name__ == '__main__': - logging.basicConfig(level=logging.DEBUG) + logging.basicConfig(level=logging.INFO) bot = Automata() + + bot.forward() + bot.right() + bot.forward() + bot.left() + bot.forward() + + exit(0) + color = COLOR_NONE + cmds = [] while color != COLOR_RED: bot.feed_tape() color = bot.get_color() - print (COLORS[color]) - break + logging.warning(COLORS[color]) + cmds.append(COLORS[color]) + + exp = ['BLUE', 'BLUE', 'BLUE', 'WHITE', 'BLUE', 'BLUE', 'WHITE', 'BLUE', 'WHITE', 'YELLOW', 'BLUE', 'BLUE', 'BLUE', + 'BLUE', 'YELLOW', 'WHITE', 'RED'] + logging.info("Exp: %s", exp) + logging.info("Act: %s", cmds) + assert exp == cmds diff --git a/examples/demo.py b/examples/demo.py index 6093875..dfaa1d8 100644 --- a/examples/demo.py +++ b/examples/demo.py @@ -4,8 +4,8 @@ from pylgbst import * from pylgbst.comms import DebugServerConnection -from pylgbst.movehub import MoveHub, COLORS, COLOR_BLACK -from pylgbst.peripherals import EncodedMotor, TiltSensor, Amperage, Voltage +from pylgbst.hub import MoveHub, math +from pylgbst.peripherals import EncodedMotor, TiltSensor, Current, Voltage, COLORS, COLOR_BLACK log = logging.getLogger("demo") @@ -95,7 +95,7 @@ def callback(pitch, roll, yaw): demo_tilt_sensor_simple.cnt += 1 log.info("Tilt #%s of %s: roll:%s pitch:%s yaw:%s", demo_tilt_sensor_simple.cnt, limit, pitch, roll, yaw) - movehub.tilt_sensor.subscribe(callback, mode=TiltSensor.MODE_3AXIS_FULL) + movehub.tilt_sensor.subscribe(callback, mode=TiltSensor.MODE_3AXIS_ACCEL) while demo_tilt_sensor_simple.cnt < limit: time.sleep(1) @@ -120,7 +120,7 @@ def callback(color, distance=None): def demo_motor_sensors(movehub): log.info("Motor rotation sensors test. Rotate all available motors once") - demo_motor_sensors.states = {movehub.motor_A: None, movehub.motor_B: None} + demo_motor_sensors.states = {movehub.motor_A: 0, movehub.motor_B: 0, movehub.motor_external: 0} def callback_a(param1): demo_motor_sensors.states[movehub.motor_A] = param1 @@ -141,7 +141,7 @@ def callback_e(param1): demo_motor_sensors.states[movehub.motor_external] = None movehub.motor_external.subscribe(callback_e) - while None in demo_motor_sensors.states.values(): + while not all([x is not None and abs(x) > 30 for x in demo_motor_sensors.states.values()]): time.sleep(1) movehub.motor_A.unsubscribe(callback_a) @@ -159,13 +159,13 @@ def callback1(value): def callback2(value): log.info("Voltage: %s", value) - movehub.amperage.subscribe(callback1, mode=Amperage.MODE1, granularity=0) - movehub.amperage.subscribe(callback1, mode=Amperage.MODE1, granularity=1) + movehub.current.subscribe(callback1, mode=Current.CURRENT_L, granularity=0) + movehub.current.subscribe(callback1, mode=Current.CURRENT_L, granularity=1) - movehub.voltage.subscribe(callback2, mode=Voltage.MODE1, granularity=0) - movehub.voltage.subscribe(callback2, mode=Voltage.MODE1, granularity=1) + movehub.voltage.subscribe(callback2, mode=Voltage.VOLTAGE_L, granularity=0) + movehub.voltage.subscribe(callback2, mode=Voltage.VOLTAGE_L, granularity=1) time.sleep(5) - movehub.amperage.unsubscribe(callback1) + movehub.current.unsubscribe(callback1) movehub.voltage.unsubscribe(callback2) @@ -184,15 +184,6 @@ def demo_all(movehub): if __name__ == '__main__': logging.basicConfig(level=logging.INFO) - try: - connection = DebugServerConnection() - except BaseException: - logging.debug("Failed to use debug server: %s", traceback.format_exc()) - connection = get_connection_auto() - - try: - hub = MoveHub(connection) - sleep(1) - # demo_all(hub) - finally: - connection.disconnect() + hub = MoveHub() + demo_all(hub) + hub.disconnect() diff --git a/examples/harmonograph/__init__.py b/examples/harmonograph/__init__.py index 8106fcf..6594591 100644 --- a/examples/harmonograph/__init__.py +++ b/examples/harmonograph/__init__.py @@ -3,20 +3,13 @@ from pylgbst import get_connection_auto from pylgbst.comms import DebugServerConnection -from pylgbst.movehub import MoveHub +from pylgbst.hub import MoveHub if __name__ == '__main__': logging.basicConfig(level=logging.INFO) - - try: - conn = DebugServerConnection() - except BaseException: - logging.warning("Failed to use debug server: %s", traceback.format_exc()) - conn = get_connection_auto() - - hub = MoveHub(conn) + hub = MoveHub() try: - hub.motor_AB.constant(0.45, 0.45) + hub.motor_AB.start_power(0.45, 0.45) hub.motor_external.angled(12590, 0.1) # time.sleep(180) finally: diff --git a/examples/plotter/__init__.py b/examples/plotter/__init__.py index 6fb13c6..8e952c5 100644 --- a/examples/plotter/__init__.py +++ b/examples/plotter/__init__.py @@ -2,8 +2,7 @@ import math import time -from pylgbst.constants import COLOR_RED, COLOR_CYAN, COLORS -from pylgbst.peripherals import ColorDistanceSensor +from pylgbst.peripherals import VisionSensor, COLOR_RED, COLOR_CYAN, COLORS class Plotter(object): @@ -13,7 +12,7 @@ class Plotter(object): def __init__(self, hub, base_speed=1.0): """ - :type hub: pylgbst.movehub.MoveHub + :type hub: pylgbst.hub.MoveHub """ self._hub = hub self.caret = self._hub.motor_A @@ -36,27 +35,27 @@ def initialize(self): self.is_tool_down = False def _reset_caret(self): - if not self._hub.color_distance_sensor: + if not self._hub.vision_sensor: logging.warning("No color/distance sensor, cannot center caret") return - self._hub.color_distance_sensor.subscribe(self._on_distance, mode=ColorDistanceSensor.COLOR_DISTANCE_FLOAT) + self._hub.vision_sensor.subscribe(self._on_distance, mode=VisionSensor.COLOR_DISTANCE_FLOAT) self.caret.timed(0.5, 1) try: - self.caret.constant(-1) + self.caret.start_power(-1) count = 0 max_tries = 50 while self._marker_color not in (COLOR_RED, COLOR_CYAN) and count < max_tries: time.sleep(30.0 / max_tries) count += 1 - self._hub.color_distance_sensor.unsubscribe(self._on_distance) + self._hub.vision_sensor.unsubscribe(self._on_distance) clr = COLORS[self._marker_color] if self._marker_color else None logging.info("Centering tries: %s, color #%s", count, clr) if count >= max_tries: raise RuntimeError("Failed to center caret") finally: self.caret.stop() - self._hub.color_distance_sensor.unsubscribe(self._on_distance) + self._hub.vision_sensor.unsubscribe(self._on_distance) if self._marker_color == COLOR_CYAN: self.move(-0.1, 0) @@ -84,7 +83,7 @@ def _compensate_wheels_backlash(self, movy): def finalize(self): if self.is_tool_down: self._tool_up() - self.both.stop(is_async=True) + self.both.stop() def _tool_down(self): self.is_tool_down = True @@ -192,7 +191,7 @@ def circle(self, radius): spa = speed_a * self.base_speed spb = -speed_b * self.base_speed * self.MOTOR_RATIO logging.info("Motor speeds: %.3f / %.3f", spa, spb) - self.both.constant(spa, spb) + self.both.start_power(spa, spb) time.sleep(dur) def spiral(self, rounds, growth): @@ -216,7 +215,7 @@ def spiral(self, rounds, growth): for speed_a, speed_b, dur in speeds: spa = speed_a * self.base_speed spb = -speed_b * self.base_speed * self.MOTOR_RATIO - self.both.constant(spa, spb) + self.both.start_power(spa, spb) logging.info("Motor speeds: %.3f / %.3f", spa, spb) time.sleep(dur) diff --git a/examples/plotter/try.py b/examples/plotter/try.py index 7b69ca8..d85eb4f 100644 --- a/examples/plotter/try.py +++ b/examples/plotter/try.py @@ -1,14 +1,9 @@ # coding=utf-8 import logging import time -import traceback - -import six from examples.plotter import Plotter -from pylgbst import get_connection_auto -from pylgbst.comms import DebugServerConnection -from pylgbst.movehub import EncodedMotor, PORT_AB, PORT_C, PORT_A, PORT_B, MoveHub +from pylgbst.hub import EncodedMotor, MoveHub from tests import HubMock @@ -91,11 +86,11 @@ def try_speeds(): speeds = [x * 1.0 / 10.0 for x in range(1, 11)] for s in speeds: logging.info("%s", s) - plotter.both.constant(s, -s) + plotter.both.start_power(s, -s) time.sleep(1) for s in reversed(speeds): logging.info("%s", s) - plotter.both.constant(-s, s) + plotter.both.start_power(-s, s) time.sleep(1) @@ -182,16 +177,15 @@ def angles_experiment(): class MotorMock(EncodedMotor): - def _wait_sync(self, is_async): - super(MotorMock, self)._wait_sync(True) + pass def get_hub_mock(): hub = HubMock() - hub.motor_A = MotorMock(hub, PORT_A) - hub.motor_B = MotorMock(hub, PORT_B) - hub.motor_AB = MotorMock(hub, PORT_AB) - hub.motor_external = MotorMock(hub, PORT_C) + hub.motor_A = MotorMock(hub, MoveHub.PORT_A) + hub.motor_B = MotorMock(hub, MoveHub.PORT_B) + hub.motor_AB = MotorMock(hub, MoveHub.PORT_AB) + hub.motor_external = MotorMock(hub, MoveHub.PORT_C) return hub @@ -220,13 +214,7 @@ def interpret_command(cmd, plotter): logging.basicConfig(level=logging.DEBUG) logging.getLogger('').setLevel(logging.DEBUG) - try: - conn = DebugServerConnection() - except BaseException: - logging.warning("Failed to use debug server: %s", traceback.format_exc()) - conn = get_connection_auto() - - hub = MoveHub(conn) if 1 else get_hub_mock() + hub = MoveHub() if 1 else get_hub_mock() plotter = Plotter(hub, 0.75) FIELD_WIDTH = 0.9 diff --git a/examples/sorter/__init__.py b/examples/sorter/__init__.py index c7ce597..7f2e67a 100644 --- a/examples/sorter/__init__.py +++ b/examples/sorter/__init__.py @@ -1,10 +1,7 @@ import logging -import traceback -from pylgbst import get_connection_auto -from pylgbst.comms import DebugServerConnection -from pylgbst.constants import COLORS, COLOR_YELLOW, COLOR_BLUE, COLOR_CYAN, COLOR_RED, COLOR_BLACK -from pylgbst.movehub import MoveHub +from pylgbst.hub import MoveHub +from pylgbst.peripherals import COLOR_YELLOW, COLOR_BLUE, COLOR_CYAN, COLOR_RED, COLOR_BLACK, COLORS class ColorSorter(MoveHub): @@ -16,7 +13,7 @@ def __init__(self, connection=None): self.color = 0 self.distance = 10 self._last_wheel_dir = 1 - self.color_distance_sensor.subscribe(self.on_color) + self.vision_sensor.subscribe(self.on_color) self.queue = [None for _ in range(0, 1)] def on_color(self, colr, dist): @@ -50,10 +47,9 @@ def move_to_bucket(self, color): self._last_wheel_dir = wheel_dir def clear(self): - self.color_distance_sensor.unsubscribe(self.on_color) - if not self.motor_B.in_progress(): - self.move_to_bucket(COLOR_BLACK) - self.motor_AB.stop(is_async=True) + self.vision_sensor.unsubscribe(self.on_color) + self.move_to_bucket(COLOR_BLACK) + self.motor_AB.stop() def tick(self): res = False @@ -80,13 +76,7 @@ def tick(self): if __name__ == '__main__': logging.basicConfig(level=logging.INFO) - try: - conn = DebugServerConnection() - except BaseException: - logging.warning("Failed to use debug server: %s", traceback.format_exc()) - conn = get_connection_auto() - - sorter = ColorSorter(conn) + sorter = ColorSorter() empty = 0 try: while True: diff --git a/examples/tracker/__init__.py b/examples/tracker/__init__.py index 0e01e33..e8b7b7a 100644 --- a/examples/tracker/__init__.py +++ b/examples/tracker/__init__.py @@ -9,10 +9,8 @@ import imutils as imutils from matplotlib import pyplot -from pylgbst import get_connection_auto -from pylgbst.comms import DebugServerConnection -from pylgbst.constants import COLOR_RED, COLOR_BLUE, COLOR_YELLOW -from pylgbst.movehub import MoveHub +from pylgbst.hub import MoveHub +from pylgbst.peripherals import COLOR_RED, COLOR_BLUE, COLOR_YELLOW cascades_dir = '/usr/share/opencv/haarcascades' face_cascade = cv2.CascadeClassifier(cascades_dir + '/haarcascade_frontalface_default.xml') @@ -85,6 +83,7 @@ def _reduce(self, values): return res def _find_smile(self, cur_face): + roi_color = None if cur_face is not None: (x, y, w, h) = cur_face roi_color = self.cur_img[y:y + h, x:x + w] @@ -114,8 +113,7 @@ 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) - if self.led.last_color_set != COLOR_RED: - self.led.set_color(COLOR_RED) + self.led.set_color(COLOR_RED) if not on and self._is_smile_on: self._is_smile_on = False @@ -142,7 +140,7 @@ def _find_color(self): mask = cv2.erode(mask, None, iterations=5) mask = cv2.dilate(mask, None, iterations=5) - #if not (int(time.time()) % 2): + # if not (int(time.time()) % 2): # self.cur_img = mask ret, thresh = cv2.threshold(mask, 20, 255, 0) @@ -167,8 +165,8 @@ def _auto_pan(self, cur_face): if abs(vert) < 0.15: vert = 0 - self.motor_external.constant(horz) - self.motor_A.constant(-vert) + self.motor_external.start_power(horz) + self.motor_A.start_power(-vert) def main(self): thr = Thread(target=self.capture) @@ -191,15 +189,15 @@ def main(self): def _process_picture(self, plt): self.cur_face = self._find_face() - #self.cur_face = self._find_color() + # self.cur_face = self._find_color() if self.cur_face is None: self.motor_external.stop() self.motor_AB.stop() - if not self._is_smile_on and self.led.last_color_set != COLOR_BLUE: + if not self._is_smile_on: self.led.set_color(COLOR_BLUE) else: - if not self._is_smile_on and self.led.last_color_set != COLOR_YELLOW: + if not self._is_smile_on: self.led.set_color(COLOR_YELLOW) self._auto_pan(self.cur_face) @@ -213,13 +211,7 @@ def _process_picture(self, plt): logging.basicConfig(level=logging.INFO) try: - conn = DebugServerConnection() - except BaseException: - logging.debug("Failed to use debug server: %s", traceback.format_exc()) - conn = get_connection_auto() - - try: - hub = FaceTracker(conn) + hub = FaceTracker() hub.main() finally: pass diff --git a/examples/vernie/__init__.py b/examples/vernie/__init__.py index 0b83f0b..4a7cc93 100644 --- a/examples/vernie/__init__.py +++ b/examples/vernie/__init__.py @@ -6,8 +6,7 @@ import time from pylgbst import * -from pylgbst.comms import DebugServerConnection -from pylgbst.movehub import MoveHub +from pylgbst.hub import MoveHub try: import gtts @@ -65,17 +64,11 @@ def say(text): class Vernie(MoveHub): def __init__(self, language='en'): - try: - conn = DebugServerConnection() - except BaseException: - logging.warning("Failed to use debug server: %s", traceback.format_exc()) - conn = get_connection_auto() - - super(Vernie, self).__init__(conn) + super(Vernie, self).__init__() self.language = language while True: - required_devices = (self.color_distance_sensor, self.motor_external) + required_devices = (self.vision_sensor, self.motor_external) if None not in required_devices: break log.debug("Waiting for required devices to appear: %s", required_devices) diff --git a/examples/vernie/android_remote.py b/examples/vernie/android_remote.py index 90dd57d..1ec4be4 100644 --- a/examples/vernie/android_remote.py +++ b/examples/vernie/android_remote.py @@ -10,7 +10,7 @@ import time from examples.vernie import Vernie -from pylgbst.peripherals import ColorDistanceSensor +from pylgbst.peripherals import VisionSensor host = '' port = 8999 @@ -54,7 +54,7 @@ def on_distance(distance): robot.button.subscribe(on_btn) robot.motor_AB.stop() -robot.color_distance_sensor.subscribe(on_distance, ColorDistanceSensor.DISTANCE_INCHES) +robot.vision_sensor.subscribe(on_distance, VisionSensor.DISTANCE_INCHES) try: udp_sock.bind((host, port)) time.sleep(1) @@ -85,7 +85,7 @@ def on_distance(distance): sa = round(c + b / divider, 1) sb = round(c - b / divider, 1) logging.info("SpeedA=%s, SpeedB=%s", sa, sb) - robot.motor_AB.constant(sa, sb) + robot.motor_AB.start_power(sa, sb) # time.sleep(0.5) finally: robot.motor_AB.stop() diff --git a/examples/vernie/go_towards_light.py b/examples/vernie/go_towards_light.py index c90e454..27ec37f 100644 --- a/examples/vernie/go_towards_light.py +++ b/examples/vernie/go_towards_light.py @@ -1,4 +1,4 @@ -from pylgbst.peripherals import ColorDistanceSensor +from pylgbst.peripherals import VisionSensor from . import * logging.basicConfig(level=logging.INFO) @@ -29,7 +29,7 @@ def on_turn(angl): robot.button.subscribe(on_btn) -robot.color_distance_sensor.subscribe(on_change_lum, ColorDistanceSensor.LUMINOSITY, granularity=1) +robot.vision_sensor.subscribe(on_change_lum, VisionSensor.DEBUG, granularity=1) robot.motor_A.subscribe(on_turn, granularity=30) # TODO: add bump detect to go back? @@ -62,5 +62,5 @@ def on_turn(angl): logging.info("Luminosity is %.3f, moving towards it", cur_luminosity) robot.move(FORWARD, 1) -robot.color_distance_sensor.unsubscribe(on_change_lum) +robot.vision_sensor.unsubscribe(on_change_lum) robot.button.unsubscribe(on_btn) diff --git a/examples/vernie/run_away_game.py b/examples/vernie/run_away_game.py index f5b5eb2..18fc4a5 100644 --- a/examples/vernie/run_away_game.py +++ b/examples/vernie/run_away_game.py @@ -23,13 +23,13 @@ def on_btn(pressed): robot.led.set_color(COLOR_GREEN) robot.button.subscribe(on_btn) -robot.color_distance_sensor.subscribe(callback) +robot.vision_sensor.subscribe(callback) robot.say("Place your hand in front of sensor") while running: time.sleep(1) -robot.color_distance_sensor.unsubscribe(callback) +robot.vision_sensor.unsubscribe(callback) robot.button.unsubscribe(on_btn) robot.led.set_color(COLOR_NONE) while robot.led.in_progress(): diff --git a/pylgbst/comms/__init__.py b/pylgbst/comms/__init__.py index 26f5576..979fd82 100644 --- a/pylgbst/comms/__init__.py +++ b/pylgbst/comms/__init__.py @@ -10,12 +10,18 @@ from binascii import unhexlify from threading import Thread -from pylgbst.constants import MSG_DEVICE_SHUTDOWN, ENABLE_NOTIFICATIONS_HANDLE, ENABLE_NOTIFICATIONS_VALUE +from pylgbst.messages import MsgHubAction from pylgbst.utilities import str2hex log = logging.getLogger('comms') LEGO_MOVE_HUB = "LEGO Move Hub" +MOVE_HUB_HW_UUID_SERV = '00001623-1212-efde-1623-785feabcd123' +MOVE_HUB_HW_UUID_CHAR = '00001624-1212-efde-1623-785feabcd123' +ENABLE_NOTIFICATIONS_HANDLE = 0x000f +ENABLE_NOTIFICATIONS_VALUE = b'\x01\x00' + +MOVE_HUB_HARDWARE_HANDLE = 0x0E class Connection(object): @@ -96,7 +102,7 @@ def _notify(self, conn, handle, data): self._check_shutdown(data) def _check_shutdown(self, data): - if data[5] == MSG_DEVICE_SHUTDOWN: + if data[5] == MsgHubAction.TYPE: log.warning("Device shutdown") self._running = False diff --git a/pylgbst/comms/cbluepy.py b/pylgbst/comms/cbluepy.py index c0d5ae4..a5ce43b 100644 --- a/pylgbst/comms/cbluepy.py +++ b/pylgbst/comms/cbluepy.py @@ -1,14 +1,10 @@ -import re import logging +import re from threading import Thread, Event -import time -from contextlib import contextmanager -from enum import Enum from bluepy import btle from pylgbst.comms import Connection, LEGO_MOVE_HUB -from pylgbst.constants import MOVE_HUB_HW_UUID_CHAR from pylgbst.utilities import str2hex, queue log = logging.getLogger('comms-bluepy') diff --git a/pylgbst/comms/cgatt.py b/pylgbst/comms/cgatt.py index b897af4..c0ce153 100644 --- a/pylgbst/comms/cgatt.py +++ b/pylgbst/comms/cgatt.py @@ -5,8 +5,8 @@ import gatt -from pylgbst.comms import Connection, LEGO_MOVE_HUB -from pylgbst.constants import MOVE_HUB_HW_UUID_SERV, MOVE_HUB_HW_UUID_CHAR, MOVE_HUB_HARDWARE_HANDLE +from pylgbst.comms import Connection, LEGO_MOVE_HUB, MOVE_HUB_HW_UUID_SERV, MOVE_HUB_HW_UUID_CHAR, \ + MOVE_HUB_HARDWARE_HANDLE from pylgbst.utilities import str2hex log = logging.getLogger('comms-gatt') diff --git a/pylgbst/comms/cpygatt.py b/pylgbst/comms/cpygatt.py index 661a2ce..ab91dce 100644 --- a/pylgbst/comms/cpygatt.py +++ b/pylgbst/comms/cpygatt.py @@ -2,8 +2,7 @@ import pygatt -from pylgbst.comms import Connection, LEGO_MOVE_HUB -from pylgbst.constants import MOVE_HUB_HW_UUID_CHAR +from pylgbst.comms import Connection, LEGO_MOVE_HUB, MOVE_HUB_HW_UUID_CHAR from pylgbst.utilities import str2hex log = logging.getLogger('comms-pygatt') diff --git a/pylgbst/constants.py b/pylgbst/constants.py deleted file mode 100644 index fb10f90..0000000 --- a/pylgbst/constants.py +++ /dev/null @@ -1,120 +0,0 @@ -# GENERAL -ENABLE_NOTIFICATIONS_HANDLE = 0x000f -ENABLE_NOTIFICATIONS_VALUE = b'\x01\x00' -MOVE_HUB_HARDWARE_HANDLE = 0x0E -MOVE_HUB_HW_UUID_SERV = '00001623-1212-efde-1623-785feabcd123' -MOVE_HUB_HW_UUID_CHAR = '00001624-1212-efde-1623-785feabcd123' - -PACKET_VER = 0x01 -LEGO_MOVE_HUB = "LEGO Move Hub" - -# PORTS -PORT_C = 0x01 -PORT_D = 0x02 -PORT_LED = 0x32 -PORT_A = 0x37 -PORT_B = 0x38 -PORT_AB = 0x39 -PORT_TILT_SENSOR = 0x3A -PORT_AMPERAGE = 0x3B -PORT_VOLTAGE = 0x3C - -PORTS = { - PORT_A: "A", - PORT_B: "B", - PORT_AB: "AB", - PORT_C: "C", - PORT_D: "D", - PORT_LED: "LED", - PORT_TILT_SENSOR: "TILT_SENSOR", - PORT_AMPERAGE: "AMPERAGE", - PORT_VOLTAGE: "VOLTAGE", -} - -# PACKET TYPES -MSG_DEVICE_INFO = 0x01 -# 0501010305 gives 090001030600000010 -MSG_DEVICE_SHUTDOWN = 0x02 # sent when hub shuts down by button hold -MSG_PING_RESPONSE = 0x03 -MSG_PORT_INFO = 0x04 -MSG_PORT_CMD_ERROR = 0x05 - - -MSG_SET_PORT_VAL = 0x81 -MSG_PORT_STATUS = 0x82 -MSG_SENSOR_SUBSCRIBE = 0x41 -MSG_SENSOR_SOMETHING1 = 0x42 # it is seen close to sensor subscribe commands. Subscription options? Initial value? -MSG_SENSOR_DATA = 0x45 -MSG_SENSOR_SUBSCRIBE_ACK = 0x47 - -# DEVICE TYPES -DEV_VOLTAGE = 0x14 -DEV_AMPERAGE = 0x15 -DEV_LED = 0x17 -DEV_DCS = 0x25 -DEV_IMOTOR = 0x26 -DEV_MOTOR = 0x27 -DEV_TILT_SENSOR = 0x28 - -DEVICE_TYPES = { - DEV_DCS: "DISTANCE_COLOR_SENSOR", - DEV_IMOTOR: "IMOTOR", - DEV_MOTOR: "MOTOR", - DEV_TILT_SENSOR: "TILT_SENSOR", - DEV_LED: "LED", - DEV_AMPERAGE: "AMPERAGE", - DEV_VOLTAGE: "VOLTAGE", -} - -# NOTIFICATIONS -STATUS_STARTED = 0x01 -STATUS_CONFLICT = 0x05 -STATUS_FINISHED = 0x0a -STATUS_INPROGRESS = 0x0c # FIXME: not sure about description -STATUS_INTERRUPTED = 0x0e # FIXME: not sure about description - -# COLORS -COLOR_BLACK = 0x00 -COLOR_PINK = 0x01 -COLOR_PURPLE = 0x02 -COLOR_BLUE = 0x03 -COLOR_LIGHTBLUE = 0x04 -COLOR_CYAN = 0x05 -COLOR_GREEN = 0x06 -COLOR_YELLOW = 0x07 -COLOR_ORANGE = 0x09 -COLOR_RED = 0x09 -COLOR_WHITE = 0x0a -COLOR_NONE = 0xFF -COLORS = { - COLOR_BLACK: "BLACK", - COLOR_PINK: "PINK", - COLOR_PURPLE: "PURPLE", - COLOR_BLUE: "BLUE", - COLOR_LIGHTBLUE: "LIGHTBLUE", - COLOR_CYAN: "CYAN", - COLOR_GREEN: "GREEN", - COLOR_YELLOW: "YELLOW", - COLOR_ORANGE: "ORANGE", - COLOR_RED: "RED", - COLOR_WHITE: "WHITE", - COLOR_NONE: "NONE" -} - -# DEVICE INFO -INFO_DEVICE_NAME = 0x01 -INFO_BUTTON_STATE = 0x02 -INFO_FIRMWARE_VERSION = 0x03 -INFO_SOME4 = 0x04 -INFO_SOME5_JITTERING = 0x05 -INFO_SOME6 = 0x06 -INFO_SOME7 = 0x07 -INFO_MANUFACTURER = 0x08 -INFO_HW_VERSION = 0x09 -INFO_SOME10 = 0x0a -INFO_SOME11 = 0x0b -INFO_SOME12 = 0x0c - -INFO_ACTION_SUBSCRIBE = 0x02 -INFO_ACTION_UNSUBSCRIBE = 0x03 -INFO_ACTION_GET = 0x05 diff --git a/pylgbst/hub.py b/pylgbst/hub.py new file mode 100644 index 0000000..04a65a8 --- /dev/null +++ b/pylgbst/hub.py @@ -0,0 +1,273 @@ +import threading +import time + +from pylgbst import get_connection_auto +from pylgbst.messages import * +from pylgbst.peripherals import * +from pylgbst.utilities import str2hex, usbyte, ushort +from pylgbst.utilities import queue + +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() + 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) + self.connection.write(self.HUB_HARDWARE_HANDLE, 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) + + resp = self._sync_replies.get() + log.debug("Fetched sync reply: %r", resp) + if isinstance(resp, MsgGenericError): + raise RuntimeError(resp.message()) + return resp + else: + 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 0x%x on port 0x%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 + """ + + # PORTS + PORT_C = 0x01 + PORT_D = 0x02 + PORT_LED = 0x32 + PORT_A = 0x37 + PORT_B = 0x38 + PORT_AB = 0x39 + PORT_TILT_SENSOR = 0x3A + PORT_CURRENT = 0x3B + PORT_VOLTAGE = 0x3C + + # noinspection PyTypeChecker + def __init__(self, connection=None): + 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, 60): + 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): + 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] diff --git a/pylgbst/messages.py b/pylgbst/messages.py new file mode 100644 index 0000000..09e5d68 --- /dev/null +++ b/pylgbst/messages.py @@ -0,0 +1,743 @@ +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 logner messages with 2-byte len" + return pack(" 90: - return val - 256 - else: - return val + # TODO: add some methods from official doc, like + # https://lego.github.io/lego-ble-wireless-protocol-docs/index.html#output-sub-command-tiltconfigimpact-impactthreshold-bumpholdoff-n-a -class ColorDistanceSensor(Peripheral): - COLOR_ONLY = 0x00 +class VisionSensor(Peripheral): + COLOR_INDEX = 0x00 DISTANCE_INCHES = 0x01 COUNT_2INCH = 0x02 - DISTANCE_HOW_CLOSE = 0x03 - DISTANCE_SUBINCH_HOW_CLOSE = 0x04 - OFF1 = 0x05 - STREAM_3_VALUES = 0x06 - OFF2 = 0x07 - COLOR_DISTANCE_FLOAT = 0x08 - LUMINOSITY = 0x09 - SOME_20BYTES = 0x0a # TODO: understand it + DISTANCE_REFLECTED = 0x03 + AMBIENT_LIGHT = 0x04 + SET_COLOR = 0x05 + COLOR_RGB = 0x06 + SET_IR_TX = 0x07 + COLOR_DISTANCE_FLOAT = 0x08 # it's not declared by dev's mode info + + DEBUG = 0x09 # first val is by fact ambient light, second is zero + CALIBRATE = 0x0a # gives constant values def __init__(self, parent, port): - super(ColorDistanceSensor, self).__init__(parent, port) + super(VisionSensor, self).__init__(parent, port) + + def subscribe(self, callback, mode=COLOR_DISTANCE_FLOAT, granularity=1): + super(VisionSensor, self).subscribe(callback, mode, granularity) + + 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: + 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 data in mode %s: %s", self._port_mode.mode, str2hex(data)) + return () - def subscribe(self, callback, mode=COLOR_DISTANCE_FLOAT, granularity=1, is_async=False): - super(ColorDistanceSensor, self).subscribe(callback, mode, granularity) + def set_color(self, color): + if color == COLOR_NONE: + color = COLOR_BLACK - def handle_port_data(self, data): - if self._port_subscription_mode == self.COLOR_DISTANCE_FLOAT: - color = usbyte(data, 4) - distance = usbyte(data, 5) - partial = usbyte(data, 7) - if partial: - distance += 1.0 / partial - self._notify_subscribers(color, float(distance)) - elif self._port_subscription_mode == self.COLOR_ONLY: - color = usbyte(data, 4) - self._notify_subscribers(color) - elif self._port_subscription_mode == self.DISTANCE_INCHES: - distance = usbyte(data, 4) - self._notify_subscribers(distance) - elif self._port_subscription_mode == self.DISTANCE_HOW_CLOSE: - distance = usbyte(data, 4) - self._notify_subscribers(distance) - elif self._port_subscription_mode == self.DISTANCE_SUBINCH_HOW_CLOSE: - distance = usbyte(data, 4) - self._notify_subscribers(distance) - elif self._port_subscription_mode == self.OFF1 or self._port_subscription_mode == self.OFF2: - log.info("Turned off led on %s", self) - elif self._port_subscription_mode == self.COUNT_2INCH: - count = unpack("