Skip to content

Commit 27e2ffe

Browse files
committed
Merge branch 'master' of github.com:undera/pylgbst
2 parents 754add6 + 1d77645 commit 27e2ffe

File tree

4 files changed

+94
-62
lines changed

4 files changed

+94
-62
lines changed

README.md

Lines changed: 3 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -17,6 +17,8 @@ If you have Vernie assembled, you might run scripts from [`examples/vernie`](exa
1717
[![Color Pin Bot](http://img.youtube.com/vi/QY6nRYXQw_U/0.jpg)](https://youtu.be/QY6nRYXQw_U)
1818
[![BB-8 Joystick](http://img.youtube.com/vi/55kE9I4IQSU/0.jpg)](https://youtu.be/55kE9I4IQSU)
1919

20+
[Dancing Vernie](https://youtu.be/Cp2gDleP8_M)
21+
2022

2123
## Features
2224

@@ -106,3 +108,4 @@ hub = MoveHub(conn)
106108
- https://github.com/nathankellenicki/node-poweredup - JavaScript version of library
107109
- https://github.com/spezifisch/sphero-python/blob/master/BB8joyDrive.py - example with another approach to bluetooth libs
108110
- https://github.com/virantha/bricknil - for the lovers of async Python, alternative implementation of library to control PoweredUp Hubs
111+
- https://virantha.github.io/bricknil/lego_api/lego.html - good infor about modes by BrickNil

pylgbst/hub.py

Lines changed: 9 additions & 5 deletions
Original file line numberDiff line numberDiff line change
@@ -10,7 +10,7 @@
1010

1111
PERIPHERAL_TYPES = {
1212
DevTypes.MOTOR: Motor,
13-
DevTypes.SYSTEM_TRAIN_MOTOR: EncodedMotor,
13+
DevTypes.SYSTEM_TRAIN_MOTOR: TrainMotor,
1414
DevTypes.MOTOR_EXTERNAL_TACHO: EncodedMotor,
1515
DevTypes.MOTOR_INTERNAL_TACHO: EncodedMotor,
1616
DevTypes.VISION_SENSOR: VisionSensor,
@@ -152,8 +152,11 @@ def _handle_action(self, msg):
152152

153153
def _handle_device_change(self, msg):
154154
if msg.event == MsgHubAttachedIO.EVENT_DETACHED:
155-
log.debug("Detaching peripheral: %s", self.peripherals[msg.port])
156-
self.peripherals.pop(msg.port)
155+
if msg.port not in self.peripherals:
156+
log.warning("Strange: got detach command for port %s that is not attached, will ignore it", msg.port)
157+
else:
158+
log.info("Detaching peripheral: %s", self.peripherals[msg.port])
159+
self.peripherals.pop(msg.port)
157160
return
158161

159162
assert msg.event in (msg.EVENT_ATTACHED, msg.EVENT_ATTACHED_VIRTUAL)
@@ -219,8 +222,8 @@ class MoveHub(Hub):
219222
:type current: Current
220223
:type voltage: Voltage
221224
:type vision_sensor: pylgbst.peripherals.VisionSensor
222-
:type port_C: Peripheral
223-
:type port_D: Peripheral
225+
:type port_C: pylgbst.peripherals.Peripheral
226+
:type port_D: pylgbst.peripherals.Peripheral
224227
:type motor_A: EncodedMotor
225228
:type motor_B: EncodedMotor
226229
:type motor_AB: EncodedMotor
@@ -335,6 +338,7 @@ def _handle_device_change(self, msg):
335338
class SmartHub(Hub):
336339
"""
337340
Class implementing Lego SmartHub specifics
341+
https://www.lego.com/en-pt/product/hub-88009
338342
339343
:type led: LEDRGB
340344
:type current: Current

pylgbst/peripherals.py

Lines changed: 67 additions & 56 deletions
Original file line numberDiff line numberDiff line change
@@ -1,5 +1,4 @@
11
import logging
2-
import math
32
import time
43
import traceback
54
from struct import pack, unpack
@@ -15,7 +14,7 @@
1514
MsgPortModeInfo,
1615
MsgPortInputFmtSingle,
1716
)
18-
from pylgbst.utilities import queue, str2hex, usbyte, ushort, usint
17+
from pylgbst.utilities import queue, str2hex, usbyte, ushort, usint, abs_scaled_100
1918

2019
log = logging.getLogger("peripherals")
2120

@@ -75,8 +74,8 @@ def __init__(self, parent, port):
7574

7675
self._incoming_port_data = queue.Queue(1) # limit 1 means we drop data if we can't handle it fast enough
7776
thr = Thread(target=self._queue_reader)
78-
thr.setDaemon(True)
79-
thr.setName("Port data queue: %s" % self)
77+
thr.daemon = True
78+
thr.name = "Port data queue: %s" % self
8079
thr.start()
8180

8281
def __repr__(self):
@@ -97,9 +96,9 @@ def set_port_mode(self, mode, send_updates=None, update_delta=None):
9796
log.debug("Implied update delta=%s", update_delta)
9897

9998
if (
100-
self._port_mode.mode == mode
101-
and self._port_mode.upd_enabled == send_updates
102-
and self._port_mode.upd_delta == update_delta
99+
self._port_mode.mode == mode
100+
and self._port_mode.upd_enabled == send_updates
101+
and self._port_mode.upd_delta == update_delta
103102
):
104103
log.debug("Already in target mode, no need to switch")
105104
return
@@ -246,10 +245,10 @@ def set_color(self, color):
246245
assert len(color) == 3, "RGB color has to have 3 values"
247246
self.set_port_mode(self.MODE_RGB)
248247
payload = (
249-
pack("<B", self.MODE_RGB)
250-
+ pack("<B", color[0])
251-
+ pack("<B", color[1])
252-
+ pack("<B", color[2])
248+
pack("<B", self.MODE_RGB)
249+
+ pack("<B", color[0])
250+
+ pack("<B", color[1])
251+
+ pack("<B", color[2])
253252
)
254253
else:
255254
if color == COLOR_NONE:
@@ -283,7 +282,7 @@ def _decode_port_data(self, msg):
283282
usbyte(msg.payload, 2),
284283
)
285284
else:
286-
return (usbyte(msg.payload, 0),)
285+
return usbyte(msg.payload, 0),
287286

288287
# Set color of the RGB LED (no getter)
289288
color = property(fset=set_color)
@@ -304,9 +303,9 @@ def set_brightness(self, brightness):
304303
:type brightness: <int> or <float>
305304
"""
306305
if (
307-
not isinstance(brightness, (int, float))
308-
or brightness > 100
309-
or brightness < 0
306+
not isinstance(brightness, (int, float))
307+
or brightness > 100
308+
or brightness < 0
310309
):
311310
raise ValueError("Brightness must be a number between 0 and 100")
312311

@@ -331,10 +330,36 @@ def brightness(self, value):
331330
self.set_brightness(value)
332331

333332
def _decode_port_data(self, msg):
334-
return (usbyte(msg.payload, 0),)
333+
return usbyte(msg.payload, 0),
335334

336335

337-
class Motor(Peripheral):
336+
class BaseMotor(Peripheral):
337+
def _write_direct_mode(self, subcmd, params):
338+
params = pack("<B", subcmd) + params
339+
msg = MsgPortOutput(self.port, MsgPortOutput.WRITE_DIRECT_MODE_DATA, params)
340+
self._send_output(msg)
341+
342+
343+
class TrainMotor(BaseMotor):
344+
"""
345+
Simple DC motor (Lego 88011).
346+
See https://github.com/undera/pylgbst/issues/129
347+
"""
348+
SUBCMD_POWER = 0x00
349+
SUBCMD_1 = 0x01 # TODO: figure out what it does. We know it's not sensor mode.
350+
351+
def power(self, param=1.0):
352+
"""
353+
Power the motor, with value -1.0..1.0
354+
"""
355+
params = pack("<b", abs_scaled_100(param))
356+
self._write_direct_mode(self.SUBCMD_POWER, params)
357+
358+
def stop(self):
359+
self.power(0)
360+
361+
362+
class Motor(BaseMotor):
338363
SUBCMD_START_POWER = 0x01
339364
SUBCMD_START_POWER_GROUPED = 0x02
340365
SUBCMD_SET_ACC_TIME = 0x05
@@ -352,27 +377,13 @@ def __init__(self, parent, port):
352377
super().__init__(parent, port)
353378
self.cmd_in_progress = False
354379

355-
def _speed_abs(self, relative):
380+
def _speed_abs(self, relative): # FIXME: it's not "speed", rather it's a "power"
356381
if relative == Motor.END_STATE_BRAKE or relative == Motor.END_STATE_HOLD:
357382
# special value for BRAKE
358383
# https://lego.github.io/lego-ble-wireless-protocol-docs/index.html#output-sub-command-startpower-power
359384
return relative
360385

361-
if relative < -1:
362-
log.warning("Speed cannot be less than -1")
363-
relative = -1
364-
365-
if relative > 1:
366-
log.warning("Speed cannot be more than 1")
367-
relative = 1
368-
369-
absolute = math.ceil(relative * 100) # scale of 100 is proven by experiments
370-
return int(absolute)
371-
372-
def _write_direct_mode(self, subcmd, params):
373-
params = pack("<B", subcmd) + params
374-
msg = MsgPortOutput(self.port, MsgPortOutput.WRITE_DIRECT_MODE_DATA, params)
375-
self._send_output(msg)
386+
return abs_scaled_100(relative)
376387

377388
def _send_cmd(self, subcmd, params, wait_complete=True):
378389
if self.virtual_ports:
@@ -530,10 +541,10 @@ def _decode_port_data(self, msg):
530541
data = msg.payload
531542
if self._port_mode.mode == self.SENSOR_ANGLE:
532543
angle = unpack("<l", data[0:4])[0]
533-
return (angle,)
544+
return angle,
534545
elif self._port_mode.mode == self.SENSOR_SPEED:
535546
speed = unpack("<b", data[0:1])[0]
536-
return (speed,)
547+
return speed,
537548
else:
538549
log.debug("Got motor sensor data while in unexpected mode: %r", self._port_mode)
539550
return ()
@@ -604,29 +615,29 @@ def _decode_port_data(self, msg):
604615
if self._port_mode.mode == self.MODE_2AXIS_ANGLE:
605616
roll = unpack("<b", data[0:1])[0]
606617
pitch = unpack("<b", data[1:2])[0]
607-
return (roll, pitch)
618+
return roll, pitch
608619
elif self._port_mode.mode == self.MODE_3AXIS_SIMPLE:
609620
state = usbyte(data, 0)
610-
return (state,)
621+
return state,
611622
elif self._port_mode.mode == self.MODE_2AXIS_SIMPLE:
612623
state = usbyte(data, 0)
613-
return (state,)
624+
return state,
614625
elif self._port_mode.mode == self.MODE_IMPACT_COUNT:
615626
bump_count = usint(data, 0)
616-
return (bump_count,)
627+
return bump_count,
617628
elif self._port_mode.mode == self.MODE_3AXIS_ACCEL:
618629
roll = unpack("<b", data[0:1])[0]
619630
pitch = unpack("<b", data[1:2])[0]
620631
yaw = unpack("<b", data[2:3])[0] # did I get the order right?
621-
return (roll, pitch, yaw)
632+
return roll, pitch, yaw
622633
elif self._port_mode.mode == self.MODE_ORIENT_CF:
623634
state = usbyte(data, 0)
624-
return (state,)
635+
return state,
625636
elif self._port_mode.mode == self.MODE_IMPACT_CF:
626637
state = usbyte(data, 0)
627-
return (state,)
638+
return state,
628639
elif self._port_mode.mode == self.MODE_CALIBRATION:
629-
return (usbyte(data, 0), usbyte(data, 1), usbyte(data, 2))
640+
return usbyte(data, 0), usbyte(data, 1), usbyte(data, 2)
630641
else:
631642
log.debug("Got tilt sensor data while in unexpected mode: %r", self._port_mode)
632643
return ()
@@ -659,35 +670,35 @@ def _decode_port_data(self, msg):
659670
data = msg.payload
660671
if self._port_mode.mode == self.COLOR_INDEX:
661672
color = usbyte(data, 0)
662-
return (color,)
673+
return color,
663674
elif self._port_mode.mode == self.COLOR_DISTANCE_FLOAT:
664675
color = usbyte(data, 0)
665676
val = usbyte(data, 1)
666677
partial = usbyte(data, 3)
667678
if partial:
668679
val += 1.0 / partial
669-
return (color, float(val))
680+
return color, float(val)
670681
elif self._port_mode.mode == self.DISTANCE_INCHES:
671682
val = usbyte(data, 0)
672-
return (val,)
683+
return val,
673684
elif self._port_mode.mode == self.DISTANCE_REFLECTED:
674685
val = usbyte(data, 0) / 100.0
675-
return (val,)
686+
return val,
676687
elif self._port_mode.mode == self.AMBIENT_LIGHT:
677688
val = usbyte(data, 0) / 100.0
678-
return (val,)
689+
return val,
679690
elif self._port_mode.mode == self.COUNT_2INCH:
680691
count = usint(data, 0)
681-
return (count,)
692+
return count,
682693
elif self._port_mode.mode == self.COLOR_RGB:
683694
val1 = int(255 * ushort(data, 0) / 1023.0)
684695
val2 = int(255 * ushort(data, 2) / 1023.0)
685696
val3 = int(255 * ushort(data, 4) / 1023.0)
686-
return (val1, val2, val3)
697+
return val1, val2, val3
687698
elif self._port_mode.mode == self.DEBUG:
688699
val1 = 10 * ushort(data, 0) / 1023.0
689700
val2 = 10 * ushort(data, 2) / 1023.0
690-
return (val1, val2)
701+
return val1, val2
691702
elif self._port_mode.mode == self.CALIBRATE:
692703
return [ushort(data, x * 2) for x in range(8)]
693704
else:
@@ -783,7 +794,7 @@ def _decode_port_data(self, msg):
783794
data = msg.payload
784795
val = ushort(data, 0)
785796
volts = 9600.0 * val / 3893.0 / 1000.0
786-
return (volts,)
797+
return volts,
787798

788799
@property
789800
def voltage(self):
@@ -807,7 +818,7 @@ def __init__(self, parent, port):
807818
def _decode_port_data(self, msg):
808819
val = ushort(msg.payload, 0)
809820
milliampers = 2444 * val / 4095.0
810-
return (milliampers,)
821+
return milliampers,
811822

812823
@property
813824
def current(self):
@@ -846,8 +857,8 @@ def _props_msg(self, msg):
846857
:type msg: MsgHubProperties
847858
"""
848859
if (
849-
msg.property == MsgHubProperties.BUTTON
850-
and msg.operation == MsgHubProperties.UPSTREAM_UPDATE
860+
msg.property == MsgHubProperties.BUTTON
861+
and msg.operation == MsgHubProperties.UPSTREAM_UPDATE
851862
):
852863
self._notify_subscribers(usbyte(msg.parameters, 0))
853864

@@ -862,7 +873,7 @@ def __init__(self, parent, port):
862873
def _decode_port_data(self, msg):
863874
# Fix temp with a small offset to get the real temperature
864875
magic_offset = 2.1
865-
return ((unpack("<h", msg.payload)[0] / 10) - magic_offset,)
876+
return (unpack("<h", msg.payload)[0] / 10) - magic_offset,
866877

867878
@property
868879
def temperature(self):

pylgbst/utilities.py

Lines changed: 15 additions & 1 deletion
Original file line numberDiff line numberDiff line change
@@ -4,6 +4,7 @@
44

55
import binascii
66
import logging
7+
import math
78
import sys
89
from struct import unpack
910

@@ -19,7 +20,7 @@
1920

2021
def check_unpack(seq, index, pattern, size):
2122
"""Check that we got size bytes, if so, unpack using pattern"""
22-
data = seq[index : index + size]
23+
data = seq[index: index + size]
2324
assert len(data) == size, "Unexpected data len %d, expected %d" % (len(data), size)
2425
return unpack(pattern, data)[0]
2526

@@ -43,3 +44,16 @@ def str2hex(data): # we need it for python 2+3 compatibility
4344
data = bytes(data, "ascii")
4445
hexed = binascii.hexlify(data)
4546
return hexed
47+
48+
49+
def abs_scaled_100(relative):
50+
if relative < -1.0:
51+
log.warning("Speed cannot be less than -1")
52+
relative = -1.0
53+
54+
if relative > 1.0:
55+
log.warning("Speed cannot be more than 1")
56+
relative = 1.0
57+
58+
absolute = math.ceil(relative * 100) # scale of 100 is proven by experiments
59+
return int(absolute)

0 commit comments

Comments
 (0)