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

Color sensor handling

Browse Source
This commit is contained in:
Andrey Pohilko 2017-09-14 22:05:42 +03:00
parent d2d8fd54a7
commit 5ca9a859c6
3 changed files with 67 additions and 21 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

13
demo.py
View File

@ -111,16 +111,17 @@ def vernie_head(movehub):
def demo_color_sensor(movehub):
log.info("Color sensor test: give it 3 things to detect color")
log.info("Color sensor test: wave your hand in front of it")
demo_color_sensor.cnt = 0
limit = 2000
def callback(color, distance):
def callback(color, distance=None, param=None):
demo_color_sensor.cnt += 1
clr = COLORS[color] if color in COLORS else None
log.info("#%s: Color %s, distance %s", demo_color_sensor.cnt, clr, distance)
clr = COLORS[color] if color in COLORS else color
log.info("#%s/%s: Color %s, distance %s, param %s", demo_color_sensor.cnt, limit, clr, distance, param)
movehub.color_distance_sensor.subscribe(callback)
while demo_color_sensor.cnt < 300:
movehub.color_distance_sensor.subscribe(callback, 0x08)
while demo_color_sensor.cnt < limit:
time.sleep(1)
movehub.color_distance_sensor.unsubscribe(callback)

13
pylgbst/constants.py
View File

@ -120,3 +120,16 @@ TILT_STATES = {
TILT_SOME1: "LEFT1",
TILT_SOME2: "SOME2",
}
# COLOR & DISTANCE SENSOR
CLR_DIST_MODE_COLOR_ONLY = 0x00
CLR_DIST_MODE_DISTANCE_INCHES = 0x01
CLR_DIST_MODE_COUNT_2INCH = 0x02
CLR_DIST_MODE_DISTANCE_HOW_CLOSE = 0x03
CLR_DIST_MODE_DISTANCE_SUBINCH_HOW_CLOSE = 0x04
CLR_DIST_MODE_OFF1 = 0x05
CLR_DIST_MODE_STREAM_3_VALUES = 0x06
CLR_DIST_MODE_OFF2 = 0x07
CLR_DIST_MODE_COLOR_DISTANCE_INCHES_SUBINCHES = 0x08
CLR_DIST_MODE_LUMINOSITY = 0x09
CLR_DIST_MODE_SOME_14BYTES = 0x0a

62
pylgbst/peripherals.py
View File

@ -147,6 +147,7 @@ class TiltSensor(Peripheral):
if not self._subscribers:
self._write_to_hub(MSG_SENSOR_SUBSCRIBE, int2byte(self.mode) + b'\x00\x00\x00' + int2byte(0))
self.mode = None
def handle_notification(self, data):
if self.mode == TILT_SENSOR_MODE_BASIC:
@ -179,8 +180,15 @@ class TiltSensor(Peripheral):
class ColorDistanceSensor(Peripheral):
def subscribe(self, callback):
params = b'\x08\x01\x00\x00\x00'
def __init__(self, parent, port):
super(ColorDistanceSensor, self).__init__(parent, port)
self.mode = None
def subscribe(self, callback, mode=CLR_DIST_MODE_COLOR_DISTANCE_INCHES_SUBINCHES, granularity=1):
self.mode = mode
params = int2byte(mode)
params += int2byte(granularity)
params += b'\x00\x00\x00'
params += int2byte(1) # enable
self._write_to_hub(MSG_SENSOR_SUBSCRIBE, params)
self._subscribers.add(callback)
@ -190,15 +198,45 @@ class ColorDistanceSensor(Peripheral):
self._subscribers.remove(callback)
if not self._subscribers:
self._write_to_hub(MSG_SENSOR_SUBSCRIBE, b'\x08\x01\x00\x00\x00' + int2byte(0))
self._write_to_hub(MSG_SENSOR_SUBSCRIBE, int2byte(self.mode) + b'\x01\x00\x00\x00' + int2byte(0))
self.mode = None
def handle_notification(self, data):
color = get_byte(data, 4)
distance = get_byte(data, 5)
partial = get_byte(data, 7)
if partial:
distance += 1.0 / partial
self._notify_subscribers(color if color != 0xFF else None, float(distance))
if self.mode == CLR_DIST_MODE_COLOR_DISTANCE_INCHES_SUBINCHES:
color = get_byte(data, 4)
distance = get_byte(data, 5)
partial = get_byte(data, 7)
if partial:
distance += 1.0 / partial
self._notify_subscribers(color if color != 0xFF else None, float(distance))
elif self.mode == CLR_DIST_MODE_COLOR_ONLY:
color = get_byte(data, 4)
self._notify_subscribers(color if color != 0xFF else None)
elif self.mode == CLR_DIST_MODE_DISTANCE_INCHES:
distance = get_byte(data, 4)
self._notify_subscribers(float(distance))
elif self.mode == CLR_DIST_MODE_DISTANCE_HOW_CLOSE:
distance = get_byte(data, 4)
self._notify_subscribers(float(distance))
elif self.mode == CLR_DIST_MODE_DISTANCE_SUBINCH_HOW_CLOSE:
distance = get_byte(data, 4)
self._notify_subscribers(float(distance))
elif self.mode == CLR_DIST_MODE_OFF1 or self.mode == CLR_DIST_MODE_OFF2:
log.info("Turned off led on %s", self)
elif self.mode == CLR_DIST_MODE_COUNT_2INCH:
count = struct.unpack("<L", data[4:8])[0] # is it all 4 bytes or just 2?
self._notify_subscribers(count)
elif self.mode == CLR_DIST_MODE_STREAM_3_VALUES:
# TODO: understand better meaning of these 3 values
val1 = struct.unpack("<H", data[4:6])[0]
val2 = struct.unpack("<H", data[6:8])[0]
val3 = struct.unpack("<H", data[8:10])[0]
self._notify_subscribers(val1, val2, val3)
elif self.mode == CLR_DIST_MODE_LUMINOSITY:
luminosity = struct.unpack("<H", data[4:6])[0]
self._notify_subscribers(luminosity)
else:
log.warning("Unhandled data in mode %s: %s", self.mode, str2hex(data))
# 0a00 41 01 01 enable
@ -209,12 +247,6 @@ class Button(Peripheral):
super(Button, self).__init__(parent, 0)
LISTEN_COLOR_SENSOR_ON_C = b' \x0a\x00 \x41\x01 \x08\x01\x00\x00\x00\x01'
LISTEN_COLOR_SENSOR_ON_D = b' \x0a\x00 \x41\x02 \x08\x01\x00\x00\x00\x01'
LISTEN_DIST_SENSOR_ON_C = b' \x0a\x00 \x41\x01 \x08\x01\x00\x00\x00\x01'
LISTEN_DIST_SENSOR_ON_D = b' \x0a\x00 \x41\x02 \x08\x01\x00\x00\x00\x01'
LISTEN_ENCODER_ON_A = b' \x0a\x00 \x41\x37 \x02\x01\x00\x00\x00\x01'
LISTEN_ENCODER_ON_B = b' \x0a\x00 \x41\x38 \x02\x01\x00\x00\x00\x01'
LISTEN_ENCODER_ON_C = b' \x0a\x00 \x41\x01 \x02\x01\x00\x00\x00\x01'