v/Lego-Boost-Python-Library
1
0
Fork 0
mirror of https://github.com/undera/pylgbst.git synced 2020-11-18 19:37:26 -08:00
Code Issues Releases Wiki Activity

Make v1.0, based on official docs (#27)

Browse Source 
* It's HUB ID

* Rename file

* Working with official doc

* Some progress

* AttachedIO msg

* device action impl

* some better device alert impl

* restructuring

* Some port commands handled

* Some command feedback waiting

* Some more request-reply things

* Some more request-reply things

* Reworked msg classes

* Getting back to UTs

* Getting back to UTs

* Facing sync lock problems

* Facing sync lock problems

* Testing it

* Covering more with tests

* handle actions

* Hub class is almost covered

* done coverage for Hub

* done coverage for MoveHub

* Button is tested

* remove debug server from examples

* Current and voltage tested

* color sensor basic test

* cover tilt sensor

* motor sensor tested

* constant motor

* motor is tested

* hold_speed impl for motor

* motor commands recorded

* some cleanup

* some cleanup

* some cleanup

* debug

* debug

* FIX a bug

* acc/dec profiles figured out

* UT motor ops

* UT motor ops

* Get rid of weird piggyback

* fix UT

* Fix encoding?

* fix test mb

* More robust

* Checked demo works

* cosmetics

* cosmetics

* Maybe better test

* fetching and decoding some device caps

* describing devs

* describing devs works

* Applying modes we've learned

* Simple and extensible dev attach

* Reworking peripherals based on modes

* Applying modes we've learned

* implemented getting sensor data

* fixed port subscribe

* Added led out cmds on vision sensor

* Worked on color-distance sensor

* Introduce some locking for consistency

* Improved it all

* Travis flags

* improve

* improve

* improve docs
This commit is contained in:
Andrey Pokhilko 2019-05-30 17:02:50 +03:00 committed by GitHub
parent 0e7457e7be
commit 32eecac1a6
No known key found for this signature in database
GPG Key ID: 4AEE18F83AFDEB23
40 changed files with 3772 additions and 1226 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

1
.gitignore vendored
View File

@ -3,3 +3,4 @@
*.pyc *.pyc
build build
*.avi *.avi
test_real.py

38
.travis.yml
View File

@ -5,33 +5,33 @@ virtualenv:
matrix: matrix:
include: include:
- os: linux - os: linux
python: 2.7 python: 2.7
- os: linux - os: linux
python: 3.4 python: 3.4
addons: addons:
apt: apt:
packages: packages:
- libboost-python-dev - libboost-python-dev
- libboost-thread-dev - libboost-thread-dev
- libbluetooth-dev - libbluetooth-dev
- libglib2.0-dev - libglib2.0-dev
- libdbus-1-dev - libdbus-1-dev
- libdbus-glib-1-dev - libdbus-glib-1-dev
- libgirepository-1.0-1 - libgirepository-1.0-1
- python-dbus - python-dbus
- python-gi - python-gi
- python3-dbus - python3-dbus
- python3-gi - python3-gi
install: 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: after_success:
- coverage report -m - coverage report -m
- codecov - codecov

268
README.md
View File

@ -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. 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) [![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) [![Laser Engraver](http://img.youtube.com/vi/ZbKmqVBBMhM/0.jpg)](https://youtu.be/ZbKmqVBBMhM)
@ -18,16 +18,17 @@ Demonstrational videos:
## Features ## Features
- auto-detect and connect to Move Hub device - auto-detect and connect to [Move Hub](docs/MoveHub.md) device
- auto-detects peripheral devices connected to Hub - auto-detects [peripheral devices](docs/Peripherals.md) connected to Hub
- constant, angled and timed movement for motors, rotation sensor subscription - constant, angled and timed movement for [motors](docs/Motor.md), rotation sensor subscription
- color & distance sensor: several modes to measure distance, color and luminosity - [vision sensor](docs/VisionSensor.md): several modes to measure distance, color and luminosity
- tilt sensor subscription: 2 axis, 3 axis, bump detect modes - [tilt sensor](docs/TiltSensor.md) subscription: 2 axis, 3 axis, bump detect modes
- LED color change - [RGB LED](docs/LED.md) color change
- push button status subscription - [push button](docs/MoveHub.md#push-button) status subscription
- battery voltage subscription available - [battery voltage and current](docs/VoltageCurrent.md) subscription available
- permanent Bluetooth connection server for faster debugging - permanent Bluetooth connection server for faster debugging
## Usage ## Usage
_Please note that this library requires one of Bluetooth backend libraries to be installed, please read section [here](#bluetooth-backend-prerequisites) for details._ _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: Then instantiate MoveHub object and start invoking its methods. Following is example to just print peripherals detected on Hub:
```python ```python
from pylgbst.movehub import MoveHub from pylgbst.hub import MoveHub
hub = MoveHub() hub = MoveHub()
for device in hub.devices: for device in hub.peripherals:
print(device) print(device)
``` ```
### Controlling Motors Each peripheral kind has own methods to do actions and/or get sensor data. See [features](#features) list for individual doc pages.
MoveHub provides motors via following fields: ## Bluetooth Backend Prerequisites
- `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
def callback(clr):
print("Color has changed: %s" % clr)
hub = MoveHub()
hub.led.subscribe(callback)
hub.led.set_color(COLOR_RED)
for color in COLORS:
hub.led.set_color(color)
time.sleep(0.5)
hub.led.set_color(COLOR_NONE)
hub.led.unsubscribe(callback)
```
Tip: blinking orange color of LED means battery is low.
### Push Button
`MoveHub` class has field `button` to subscribe to button press and release events.
Note that `Button` class is not real `Peripheral`, as it has no port and not listed in `devices` field of Hub. Still, subscribing to button is done usual way:
```python
from pylgbst.movehub import MoveHub
def callback(is_pressed):
print("Btn pressed: %s" % is_pressed)
hub = MoveHub()
hub.button.subscribe(callback)
```
### Power Voltage & Battery
`MoveHub` class has field `voltage` to subscribe to battery voltage status. Callback accepts single parameter with current value. The range of values is float between `0` and `1.0`. Every time data is received, value is also written into `last_value` field of `Voltage` object. Values less than `0.2` are known as lowest values, when unit turns off.
```python
from pylgbst.movehub import MoveHub
import time
def callback(value):
print("Voltage: %s" % value)
hub = MoveHub()
hub.voltage.subscribe(callback)
time.sleep(1)
print ("Value: " % hub.voltage.last_value)
```
## General Notes
### Bluetooth Backend Prerequisites
You have following options to install as Bluetooth backend: You have following options to install as Bluetooth backend:
@ -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. 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 ```python
from pylgbst.movehub import MoveHub from pylgbst.hub import MoveHub
from pylgbst.comms.cgatt import GattConnection from pylgbst.comms.cgatt import GattConnection
conn = GattConnection("hci1") 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) 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 ## 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. 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 ## 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 - document all API methods
- make sure unit tests cover all important code
- make debug server to re-establish BLE connection on loss - make debug server to re-establish BLE connection on loss
## Links ## Links
- https://github.com/LEGO/lego-ble-wireless-protocol-docs - true docs for LEGO BLE protocol - 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/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/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

26
docs/GenericHub.md Normal file
View File

@ -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.

31
docs/LED.md Normal file
View File

@ -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.

55
docs/Motor.md Normal file
View File

@ -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)
```

36
docs/MoveHub.md Normal file
View File

@ -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)
```

24
docs/Peripherals.md Normal file
View File

@ -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.

42
docs/TiltSensor.md Normal file
View File

@ -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"

36
docs/VisionSensor.md Normal file
View File

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

14
docs/VoltageCurrent.md Normal file
View File

@ -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))
```

58
examples/automata/__init__.py
View File

@ -2,27 +2,31 @@ import logging
import time import time
from collections import Counter from collections import Counter
from pylgbst.movehub import MoveHub, COLOR_NONE, COLOR_CYAN, COLOR_BLUE, COLOR_BLACK, COLOR_RED, COLORS from pylgbst.hub import MoveHub, COLOR_NONE, COLOR_BLACK, COLORS, COLOR_CYAN, COLOR_BLUE, COLOR_RED
from pylgbst.peripherals import ColorDistanceSensor from pylgbst.peripherals import EncodedMotor
class Automata(object): class Automata(object):
BASE_SPEED = 0.5
def __init__(self): def __init__(self):
super(Automata, self).__init__() super(Automata, self).__init__()
self.__hub = MoveHub() 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 = [] self._sensor = []
def __on_sensor(self, color, distance=-1): 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): if color not in (COLOR_NONE, COLOR_BLACK):
self._sensor.append((color, int(distance))) self._sensor.append((color, int(distance)))
logging.info("Sensor data: %s", COLORS[color]) logging.debug("Sensor data: %s", COLORS[color])
def feed_tape(self): def feed_tape(self):
self.__hub.motor_external.angled(120, 0.25) self.__hub.motor_external.angled(60, 0.5)
time.sleep(0.2) time.sleep(0.1)
self.__hub.motor_external.angled(60, 0.5)
time.sleep(0.1)
def get_color(self): def get_color(self):
res = self._sensor res = self._sensor
@ -32,33 +36,47 @@ class Automata(object):
clr = cnts.most_common(1)[0][0] if cnts else COLOR_NONE clr = cnts.most_common(1)[0][0] if cnts else COLOR_NONE
if clr == COLOR_CYAN: if clr == COLOR_CYAN:
clr = COLOR_BLUE clr = COLOR_BLUE
elif clr == COLOR_BLACK:
clr = COLOR_NONE
return clr return clr
def left(self): def left(self):
self.__hub.motor_AB.angled(270, 0.25, -0.25) self.__hub.motor_A.angled(270, self.BASE_SPEED, -self.BASE_SPEED, end_state=EncodedMotor.END_STATE_HOLD)
time.sleep(0.5) time.sleep(0.1)
self.__hub.motor_A.stop()
def right(self): def right(self):
self.__hub.motor_AB.angled(-270, 0.25, -0.25) self.__hub.motor_B.angled(-320, self.BASE_SPEED, -self.BASE_SPEED, end_state=EncodedMotor.END_STATE_HOLD)
time.sleep(0.5) time.sleep(0.1)
self.__hub.motor_B.stop()
def forward(self): def forward(self):
self.__hub.motor_AB.angled(830, 0.25) self.__hub.motor_AB.angled(450, self.BASE_SPEED)
time.sleep(0.5)
def backward(self): def backward(self):
self.__hub.motor_AB.angled(830, -0.25) self.__hub.motor_AB.angled(-450, self.BASE_SPEED)
time.sleep(0.5)
if __name__ == '__main__': if __name__ == '__main__':
logging.basicConfig(level=logging.DEBUG) logging.basicConfig(level=logging.INFO)
bot = Automata() bot = Automata()
bot.forward()
bot.right()
bot.forward()
bot.left()
bot.forward()
exit(0)
color = COLOR_NONE color = COLOR_NONE
cmds = []
while color != COLOR_RED: while color != COLOR_RED:
bot.feed_tape() bot.feed_tape()
color = bot.get_color() color = bot.get_color()
print (COLORS[color]) logging.warning(COLORS[color])
break 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

35
examples/demo.py
View File

@ -4,8 +4,8 @@ from time import sleep
from pylgbst import * from pylgbst import *
from pylgbst.comms import DebugServerConnection from pylgbst.comms import DebugServerConnection
from pylgbst.movehub import MoveHub, COLORS, COLOR_BLACK from pylgbst.hub import MoveHub, math
from pylgbst.peripherals import EncodedMotor, TiltSensor, Amperage, Voltage from pylgbst.peripherals import EncodedMotor, TiltSensor, Current, Voltage, COLORS, COLOR_BLACK
log = logging.getLogger("demo") log = logging.getLogger("demo")
@ -95,7 +95,7 @@ def demo_tilt_sensor_precise(movehub):
demo_tilt_sensor_simple.cnt += 1 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) 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: while demo_tilt_sensor_simple.cnt < limit:
time.sleep(1) time.sleep(1)
@ -120,7 +120,7 @@ def demo_color_sensor(movehub):
def demo_motor_sensors(movehub): def demo_motor_sensors(movehub):
log.info("Motor rotation sensors test. Rotate all available motors once") 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): def callback_a(param1):
demo_motor_sensors.states[movehub.motor_A] = param1 demo_motor_sensors.states[movehub.motor_A] = param1
@ -141,7 +141,7 @@ def demo_motor_sensors(movehub):
demo_motor_sensors.states[movehub.motor_external] = None demo_motor_sensors.states[movehub.motor_external] = None
movehub.motor_external.subscribe(callback_e) 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) time.sleep(1)
movehub.motor_A.unsubscribe(callback_a) movehub.motor_A.unsubscribe(callback_a)
@ -159,13 +159,13 @@ def demo_voltage(movehub):
def callback2(value): def callback2(value):
log.info("Voltage: %s", value) log.info("Voltage: %s", value)
movehub.amperage.subscribe(callback1, mode=Amperage.MODE1, granularity=0) movehub.current.subscribe(callback1, mode=Current.CURRENT_L, granularity=0)
movehub.amperage.subscribe(callback1, mode=Amperage.MODE1, granularity=1) 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.VOLTAGE_L, granularity=0)
movehub.voltage.subscribe(callback2, mode=Voltage.MODE1, granularity=1) movehub.voltage.subscribe(callback2, mode=Voltage.VOLTAGE_L, granularity=1)
time.sleep(5) time.sleep(5)
movehub.amperage.unsubscribe(callback1) movehub.current.unsubscribe(callback1)
movehub.voltage.unsubscribe(callback2) movehub.voltage.unsubscribe(callback2)
@ -184,15 +184,6 @@ def demo_all(movehub):
if __name__ == '__main__': if __name__ == '__main__':
logging.basicConfig(level=logging.INFO) logging.basicConfig(level=logging.INFO)
try: hub = MoveHub()
connection = DebugServerConnection() demo_all(hub)
except BaseException: hub.disconnect()
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()

13
examples/harmonograph/__init__.py
View File

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

21
examples/plotter/__init__.py
View File

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

30
examples/plotter/try.py
View File

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

24
examples/sorter/__init__.py
View File

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

30
examples/tracker/__init__.py
View File

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

13
examples/vernie/__init__.py
View File

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

6
examples/vernie/android_remote.py
View File

@ -10,7 +10,7 @@ import socket
import time import time
from examples.vernie import Vernie from examples.vernie import Vernie
from pylgbst.peripherals import ColorDistanceSensor from pylgbst.peripherals import VisionSensor
host = '' host = ''
port = 8999 port = 8999
@ -54,7 +54,7 @@ robot = Vernie()
robot.button.subscribe(on_btn) robot.button.subscribe(on_btn)
robot.motor_AB.stop() robot.motor_AB.stop()
robot.color_distance_sensor.subscribe(on_distance, ColorDistanceSensor.DISTANCE_INCHES) robot.vision_sensor.subscribe(on_distance, VisionSensor.DISTANCE_INCHES)
try: try:
udp_sock.bind((host, port)) udp_sock.bind((host, port))
time.sleep(1) time.sleep(1)
@ -85,7 +85,7 @@ try:
sa = round(c + b / divider, 1) sa = round(c + b / divider, 1)
sb = round(c - b / divider, 1) sb = round(c - b / divider, 1)
logging.info("SpeedA=%s, SpeedB=%s", sa, sb) 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) # time.sleep(0.5)
finally: finally:
robot.motor_AB.stop() robot.motor_AB.stop()

6
examples/vernie/go_towards_light.py
View File

@ -1,4 +1,4 @@
from pylgbst.peripherals import ColorDistanceSensor from pylgbst.peripherals import VisionSensor
from . import * from . import *
logging.basicConfig(level=logging.INFO) logging.basicConfig(level=logging.INFO)
@ -29,7 +29,7 @@ def on_turn(angl):
robot.button.subscribe(on_btn) 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) robot.motor_A.subscribe(on_turn, granularity=30)
# TODO: add bump detect to go back? # TODO: add bump detect to go back?
@ -62,5 +62,5 @@ while running:
logging.info("Luminosity is %.3f, moving towards it", cur_luminosity) logging.info("Luminosity is %.3f, moving towards it", cur_luminosity)
robot.move(FORWARD, 1) robot.move(FORWARD, 1)
robot.color_distance_sensor.unsubscribe(on_change_lum) robot.vision_sensor.unsubscribe(on_change_lum)
robot.button.unsubscribe(on_btn) robot.button.unsubscribe(on_btn)

4
examples/vernie/run_away_game.py
View File

@ -23,13 +23,13 @@ def on_btn(pressed):
robot.led.set_color(COLOR_GREEN) robot.led.set_color(COLOR_GREEN)
robot.button.subscribe(on_btn) 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") robot.say("Place your hand in front of sensor")
while running: while running:
time.sleep(1) time.sleep(1)
robot.color_distance_sensor.unsubscribe(callback) robot.vision_sensor.unsubscribe(callback)
robot.button.unsubscribe(on_btn) robot.button.unsubscribe(on_btn)
robot.led.set_color(COLOR_NONE) robot.led.set_color(COLOR_NONE)
while robot.led.in_progress(): while robot.led.in_progress():

10
pylgbst/comms/__init__.py
View File

@ -10,12 +10,18 @@ from abc import abstractmethod
from binascii import unhexlify from binascii import unhexlify
from threading import Thread 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 from pylgbst.utilities import str2hex
log = logging.getLogger('comms') log = logging.getLogger('comms')
LEGO_MOVE_HUB = "LEGO Move Hub" 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): class Connection(object):
@ -96,7 +102,7 @@ class DebugServer(object):
self._check_shutdown(data) self._check_shutdown(data)
def _check_shutdown(self, data): def _check_shutdown(self, data):
if data[5] == MSG_DEVICE_SHUTDOWN: if data[5] == MsgHubAction.TYPE:
log.warning("Device shutdown") log.warning("Device shutdown")
self._running = False self._running = False

6
pylgbst/comms/cbluepy.py
View File

@ -1,14 +1,10 @@
import re
import logging import logging
import re
from threading import Thread, Event from threading import Thread, Event
import time
from contextlib import contextmanager
from enum import Enum
from bluepy import btle from bluepy import btle
from pylgbst.comms import Connection, LEGO_MOVE_HUB from pylgbst.comms import Connection, LEGO_MOVE_HUB
from pylgbst.constants import MOVE_HUB_HW_UUID_CHAR
from pylgbst.utilities import str2hex, queue from pylgbst.utilities import str2hex, queue
log = logging.getLogger('comms-bluepy') log = logging.getLogger('comms-bluepy')

4
pylgbst/comms/cgatt.py
View File

@ -5,8 +5,8 @@ from time import sleep
import gatt import gatt
from pylgbst.comms import Connection, LEGO_MOVE_HUB from pylgbst.comms import Connection, LEGO_MOVE_HUB, MOVE_HUB_HW_UUID_SERV, MOVE_HUB_HW_UUID_CHAR, \
from pylgbst.constants import MOVE_HUB_HW_UUID_SERV, MOVE_HUB_HW_UUID_CHAR, MOVE_HUB_HARDWARE_HANDLE MOVE_HUB_HARDWARE_HANDLE
from pylgbst.utilities import str2hex from pylgbst.utilities import str2hex
log = logging.getLogger('comms-gatt') log = logging.getLogger('comms-gatt')

3
pylgbst/comms/cpygatt.py
View File

@ -2,8 +2,7 @@ import logging
import pygatt import pygatt
from pylgbst.comms import Connection, LEGO_MOVE_HUB from pylgbst.comms import Connection, LEGO_MOVE_HUB, MOVE_HUB_HW_UUID_CHAR
from pylgbst.constants import MOVE_HUB_HW_UUID_CHAR
from pylgbst.utilities import str2hex from pylgbst.utilities import str2hex
log = logging.getLogger('comms-pygatt') log = logging.getLogger('comms-pygatt')

120
pylgbst/constants.py
View File

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

273
pylgbst/hub.py Normal file
View File

@ -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]

743
pylgbst/messages.py Normal file
View File

@ -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("<B", msglen) + pack("<B", self.hub_id) + pack("<B", self.TYPE) + self.payload
def __repr__(self):
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', 'needs_reply')}
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)
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 == self.UPD_REQUEST:
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):
assert isinstance(msg, MsgHubAction)
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
)

250
pylgbst/movehub.py
View File

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

753
pylgbst/peripherals.py
View File

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

12
pylgbst/utilities.py
View File

@ -3,6 +3,7 @@ This module offers some utilities, in a way they are work in both Python 2 and 3
""" """
import binascii import binascii
import logging
import sys import sys
from struct import unpack from struct import unpack
@ -22,5 +23,14 @@ def ushort(seq, index):
return unpack("<H", seq[index:index + 2])[0] return unpack("<H", seq[index:index + 2])[0]
def usint(seq, index):
return unpack("<I", seq[index:index + 4])[0]
def str2hex(data): # we need it for python 2+3 compatibility def str2hex(data): # we need it for python 2+3 compatibility
return binascii.hexlify(data).decode("utf8") # if sys.version_info[0] == 3:
# data = bytes(data, 'ascii')
if not isinstance(data, bytes):
data = bytes(data, 'ascii')
hexed = binascii.hexlify(data)
return hexed

66
tests/__init__.py
View File

@ -1,7 +1,8 @@
import time
from binascii import unhexlify from binascii import unhexlify
from pylgbst.comms import Connection from pylgbst.comms import Connection
from pylgbst.movehub import MoveHub from pylgbst.hub import MoveHub, Hub
from pylgbst.peripherals import * from pylgbst.peripherals import *
logging.basicConfig(level=logging.DEBUG) logging.basicConfig(level=logging.DEBUG)
@ -9,22 +10,18 @@ logging.basicConfig(level=logging.DEBUG)
log = logging.getLogger('test') log = logging.getLogger('test')
class HubMock(MoveHub): class HubMock(Hub):
"""
:type connection: ConnectionMock
"""
# noinspection PyUnresolvedReferences # noinspection PyUnresolvedReferences
def __init__(self, connection=None): def __init__(self, conn=None):
""" super(HubMock, self).__init__(conn if conn else ConnectionMock())
:type connection: ConnectionMock self.connection = self.connection
"""
super(HubMock, self).__init__(connection if connection else ConnectionMock())
self.notify_mock = self.connection.notifications self.notify_mock = self.connection.notifications
self.writes = self.connection.writes self.writes = self.connection.writes
def _wait_for_devices(self):
pass
def _report_status(self):
pass
class ConnectionMock(Connection): class ConnectionMock(Connection):
""" """
@ -39,22 +36,51 @@ class ConnectionMock(Connection):
self.running = True self.running = True
self.finished = False self.finished = False
self.thr = Thread(target=self.notifier)
self.thr.setDaemon(True)
def set_notify_handler(self, handler): def set_notify_handler(self, handler):
self.notification_handler = handler self.notification_handler = handler
thr = Thread(target=self.notifier) self.thr.start()
thr.setDaemon(True)
thr.start()
def notifier(self): def notifier(self):
while self.running: while self.running or self.notifications:
if self.notification_handler: if self.notification_handler:
while self.notifications: while self.notifications:
handle, data = self.notifications.pop(0) data = self.notifications.pop(0)
self.notification_handler(handle, unhexlify(data.replace(' ', ''))) s = unhexlify(data.replace(' ', ''))
time.sleep(0.1) self.notification_handler(MoveHub.HUB_HARDWARE_HANDLE, bytes(s))
time.sleep(0.01)
self.finished = True self.finished = True
def write(self, handle, data): def write(self, handle, data):
log.debug("Writing to %s: %s", handle, str2hex(data)) log.debug("Writing to %s: %s", handle, str2hex(data))
self.writes.append((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

1410
tests/descr.json Normal file
View File

File diff suppressed because it is too large Load Diff

4
tests/test_cbluepy.py
View File

@ -1,5 +1,4 @@
import unittest import unittest
import time
import pylgbst.comms.cbluepy as bp_backend import pylgbst.comms.cbluepy as bp_backend
@ -20,6 +19,7 @@ class PeripheralMock(object):
def disconnect(self): def disconnect(self):
pass pass
bp_backend.PROPAGATE_DISPATCHER_EXCEPTION = True bp_backend.PROPAGATE_DISPATCHER_EXCEPTION = True
bp_backend.btle.Peripheral = lambda *args, **kwargs: PeripheralMock(*args, **kwargs) bp_backend.btle.Peripheral = lambda *args, **kwargs: PeripheralMock(*args, **kwargs)
@ -37,6 +37,7 @@ class BluepyTestCase(unittest.TestCase):
def test_delegate(self): def test_delegate(self):
def _handler(handle, data): def _handler(handle, data):
_handler.called = True _handler.called = True
delegate = bp_backend.BluepyDelegate(_handler) delegate = bp_backend.BluepyDelegate(_handler)
delegate.handleNotification(123, 'qwe') delegate.handleNotification(123, 'qwe')
self.assertEqual(_handler.called, True) self.assertEqual(_handler.called, True)
@ -58,4 +59,3 @@ class BluepyTestCase(unittest.TestCase):
tp._dispatcher_thread.join(2) tp._dispatcher_thread.join(2)
self.assertEqual(tp._dispatcher_thread.is_alive(), False) self.assertEqual(tp._dispatcher_thread.is_alive(), False)

2
tests/test_gatt.py
View File

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

145
tests/test_hub.py Normal file
View File

@ -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 01 0125000000001000000010')
conn.notifications.append('0f00 04 02 0126000000001000000010')
conn.notifications.append('0f00 04 37 0127000100000001000000')
conn.notifications.append('0f00 04 38 0127000100000001000000')
conn.notifications.append('0900 04 39 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', 0.1)
conn.notification_delayed('0b00010d06001653a0d1d4', 0.3)
conn.notification_delayed('060001060600', 0.5)
conn.notification_delayed('0600030104ff', 0.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])

141
tests/test_movehub.py
View File

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

241
tests/test_peripherals.py Normal file
View File

@ -0,0 +1,241 @@
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)
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([False, True, False], 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('050082020a', 0.1)
motor.start_power(1.0)
self.assertEqual(b"0800810211510164", hub.writes.pop(1)[1])
hub.connection.notification_delayed('050082020a', 0.1)
motor.stop()
self.assertEqual(b"090081021107006403", hub.writes.pop(1)[1])
hub.connection.notification_delayed('050082020a', 0.1)
motor.set_acc_profile(1.0)
self.assertEqual(b"090081021105e80300", hub.writes.pop(1)[1])
hub.connection.notification_delayed('050082020a', 0.1)
motor.set_dec_profile(1.0)
self.assertEqual(b"090081021106e80300", hub.writes.pop(1)[1])
hub.connection.notification_delayed('050082020a', 0.1)
motor.start_speed(1.0)
self.assertEqual(b"090081021107646403", hub.writes.pop(1)[1])
hub.connection.notification_delayed('050082020a', 0.1)
motor.stop()
self.assertEqual(b"090081021107006403", hub.writes.pop(1)[1])
logging.debug("\n\n")
hub.connection.notification_delayed('0500820201', 0.1)
hub.connection.notification_delayed('050082020a', 0.2)
motor.timed(1.0)
self.assertEqual(b"0c0081021109e80364647f03", hub.writes.pop(1)[1])
hub.connection.notification_delayed('0500820201', 0.1)
hub.connection.notification_delayed('050082020a', 0.2)
motor.angled(180)
self.assertEqual(b"0e008102110bb400000064647f03", hub.writes.pop(1)[1])
hub.connection.notification_delayed('050082020a', 0.2)
motor.preset_encoder(-180)
self.assertEqual(b"0b0081021151024cffffff", hub.writes.pop(1)[1])
hub.connection.notification_delayed('0500820201', 0.1)
hub.connection.notification_delayed('050082020a', 0.2)
motor.goto_position(0)
self.assertEqual(b"0e008102110d00000000647f6403", 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('0a004701020100000001', 0.1)
motor.subscribe(callback)
hub.connection.notification_delayed("0800450100000000", 0.1)
hub.connection.notification_delayed("08004501ffffffff", 0.2)
hub.connection.notification_delayed("08004501feffffff", 0.3)
time.sleep(0.4)
hub.connection.notification_delayed('0a004701020000000000', 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 4701080100000001', 0.1)
cds.subscribe(callback)
hub.connection.notification_delayed("08004501ff0aff00", 0.1)
time.sleep(0.2)
hub.connection.notification_delayed('0a00 4701090100000001', 0.1)
cds.unsubscribe(callback)
hub.connection.wait_notifications_handled()
self.assertEqual([(255, 10.0)], vals)

4
tests/test_pygatt.py
View File

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