1
1
import logging
2
- import math
3
2
import time
4
3
import traceback
5
4
from struct import pack , unpack
15
14
MsgPortModeInfo ,
16
15
MsgPortInputFmtSingle ,
17
16
)
18
- from pylgbst .utilities import queue , str2hex , usbyte , ushort , usint
17
+ from pylgbst .utilities import queue , str2hex , usbyte , ushort , usint , abs_scaled_100
19
18
20
19
log = logging .getLogger ("peripherals" )
21
20
@@ -75,8 +74,8 @@ def __init__(self, parent, port):
75
74
76
75
self ._incoming_port_data = queue .Queue (1 ) # limit 1 means we drop data if we can't handle it fast enough
77
76
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
80
79
thr .start ()
81
80
82
81
def __repr__ (self ):
@@ -97,9 +96,9 @@ def set_port_mode(self, mode, send_updates=None, update_delta=None):
97
96
log .debug ("Implied update delta=%s" , update_delta )
98
97
99
98
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
103
102
):
104
103
log .debug ("Already in target mode, no need to switch" )
105
104
return
@@ -246,10 +245,10 @@ def set_color(self, color):
246
245
assert len (color ) == 3 , "RGB color has to have 3 values"
247
246
self .set_port_mode (self .MODE_RGB )
248
247
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 ])
253
252
)
254
253
else :
255
254
if color == COLOR_NONE :
@@ -283,7 +282,7 @@ def _decode_port_data(self, msg):
283
282
usbyte (msg .payload , 2 ),
284
283
)
285
284
else :
286
- return ( usbyte (msg .payload , 0 ),)
285
+ return usbyte (msg .payload , 0 ),
287
286
288
287
# Set color of the RGB LED (no getter)
289
288
color = property (fset = set_color )
@@ -304,9 +303,9 @@ def set_brightness(self, brightness):
304
303
:type brightness: <int> or <float>
305
304
"""
306
305
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
310
309
):
311
310
raise ValueError ("Brightness must be a number between 0 and 100" )
312
311
@@ -331,10 +330,36 @@ def brightness(self, value):
331
330
self .set_brightness (value )
332
331
333
332
def _decode_port_data (self , msg ):
334
- return ( usbyte (msg .payload , 0 ),)
333
+ return usbyte (msg .payload , 0 ),
335
334
336
335
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 ):
338
363
SUBCMD_START_POWER = 0x01
339
364
SUBCMD_START_POWER_GROUPED = 0x02
340
365
SUBCMD_SET_ACC_TIME = 0x05
@@ -352,27 +377,13 @@ def __init__(self, parent, port):
352
377
super ().__init__ (parent , port )
353
378
self .cmd_in_progress = False
354
379
355
- def _speed_abs (self , relative ):
380
+ def _speed_abs (self , relative ): # FIXME: it's not "speed", rather it's a "power"
356
381
if relative == Motor .END_STATE_BRAKE or relative == Motor .END_STATE_HOLD :
357
382
# special value for BRAKE
358
383
# https://lego.github.io/lego-ble-wireless-protocol-docs/index.html#output-sub-command-startpower-power
359
384
return relative
360
385
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 )
376
387
377
388
def _send_cmd (self , subcmd , params , wait_complete = True ):
378
389
if self .virtual_ports :
@@ -530,10 +541,10 @@ def _decode_port_data(self, msg):
530
541
data = msg .payload
531
542
if self ._port_mode .mode == self .SENSOR_ANGLE :
532
543
angle = unpack ("<l" , data [0 :4 ])[0 ]
533
- return ( angle ,)
544
+ return angle ,
534
545
elif self ._port_mode .mode == self .SENSOR_SPEED :
535
546
speed = unpack ("<b" , data [0 :1 ])[0 ]
536
- return ( speed ,)
547
+ return speed ,
537
548
else :
538
549
log .debug ("Got motor sensor data while in unexpected mode: %r" , self ._port_mode )
539
550
return ()
@@ -604,29 +615,29 @@ def _decode_port_data(self, msg):
604
615
if self ._port_mode .mode == self .MODE_2AXIS_ANGLE :
605
616
roll = unpack ("<b" , data [0 :1 ])[0 ]
606
617
pitch = unpack ("<b" , data [1 :2 ])[0 ]
607
- return ( roll , pitch )
618
+ return roll , pitch
608
619
elif self ._port_mode .mode == self .MODE_3AXIS_SIMPLE :
609
620
state = usbyte (data , 0 )
610
- return ( state ,)
621
+ return state ,
611
622
elif self ._port_mode .mode == self .MODE_2AXIS_SIMPLE :
612
623
state = usbyte (data , 0 )
613
- return ( state ,)
624
+ return state ,
614
625
elif self ._port_mode .mode == self .MODE_IMPACT_COUNT :
615
626
bump_count = usint (data , 0 )
616
- return ( bump_count ,)
627
+ return bump_count ,
617
628
elif self ._port_mode .mode == self .MODE_3AXIS_ACCEL :
618
629
roll = unpack ("<b" , data [0 :1 ])[0 ]
619
630
pitch = unpack ("<b" , data [1 :2 ])[0 ]
620
631
yaw = unpack ("<b" , data [2 :3 ])[0 ] # did I get the order right?
621
- return ( roll , pitch , yaw )
632
+ return roll , pitch , yaw
622
633
elif self ._port_mode .mode == self .MODE_ORIENT_CF :
623
634
state = usbyte (data , 0 )
624
- return ( state ,)
635
+ return state ,
625
636
elif self ._port_mode .mode == self .MODE_IMPACT_CF :
626
637
state = usbyte (data , 0 )
627
- return ( state ,)
638
+ return state ,
628
639
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 )
630
641
else :
631
642
log .debug ("Got tilt sensor data while in unexpected mode: %r" , self ._port_mode )
632
643
return ()
@@ -659,35 +670,35 @@ def _decode_port_data(self, msg):
659
670
data = msg .payload
660
671
if self ._port_mode .mode == self .COLOR_INDEX :
661
672
color = usbyte (data , 0 )
662
- return ( color ,)
673
+ return color ,
663
674
elif self ._port_mode .mode == self .COLOR_DISTANCE_FLOAT :
664
675
color = usbyte (data , 0 )
665
676
val = usbyte (data , 1 )
666
677
partial = usbyte (data , 3 )
667
678
if partial :
668
679
val += 1.0 / partial
669
- return ( color , float (val ) )
680
+ return color , float (val )
670
681
elif self ._port_mode .mode == self .DISTANCE_INCHES :
671
682
val = usbyte (data , 0 )
672
- return ( val ,)
683
+ return val ,
673
684
elif self ._port_mode .mode == self .DISTANCE_REFLECTED :
674
685
val = usbyte (data , 0 ) / 100.0
675
- return ( val ,)
686
+ return val ,
676
687
elif self ._port_mode .mode == self .AMBIENT_LIGHT :
677
688
val = usbyte (data , 0 ) / 100.0
678
- return ( val ,)
689
+ return val ,
679
690
elif self ._port_mode .mode == self .COUNT_2INCH :
680
691
count = usint (data , 0 )
681
- return ( count ,)
692
+ return count ,
682
693
elif self ._port_mode .mode == self .COLOR_RGB :
683
694
val1 = int (255 * ushort (data , 0 ) / 1023.0 )
684
695
val2 = int (255 * ushort (data , 2 ) / 1023.0 )
685
696
val3 = int (255 * ushort (data , 4 ) / 1023.0 )
686
- return ( val1 , val2 , val3 )
697
+ return val1 , val2 , val3
687
698
elif self ._port_mode .mode == self .DEBUG :
688
699
val1 = 10 * ushort (data , 0 ) / 1023.0
689
700
val2 = 10 * ushort (data , 2 ) / 1023.0
690
- return ( val1 , val2 )
701
+ return val1 , val2
691
702
elif self ._port_mode .mode == self .CALIBRATE :
692
703
return [ushort (data , x * 2 ) for x in range (8 )]
693
704
else :
@@ -783,7 +794,7 @@ def _decode_port_data(self, msg):
783
794
data = msg .payload
784
795
val = ushort (data , 0 )
785
796
volts = 9600.0 * val / 3893.0 / 1000.0
786
- return ( volts ,)
797
+ return volts ,
787
798
788
799
@property
789
800
def voltage (self ):
@@ -807,7 +818,7 @@ def __init__(self, parent, port):
807
818
def _decode_port_data (self , msg ):
808
819
val = ushort (msg .payload , 0 )
809
820
milliampers = 2444 * val / 4095.0
810
- return ( milliampers ,)
821
+ return milliampers ,
811
822
812
823
@property
813
824
def current (self ):
@@ -846,8 +857,8 @@ def _props_msg(self, msg):
846
857
:type msg: MsgHubProperties
847
858
"""
848
859
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
851
862
):
852
863
self ._notify_subscribers (usbyte (msg .parameters , 0 ))
853
864
@@ -862,7 +873,7 @@ def __init__(self, parent, port):
862
873
def _decode_port_data (self , msg ):
863
874
# Fix temp with a small offset to get the real temperature
864
875
magic_offset = 2.1
865
- return (( unpack ("<h" , msg .payload )[0 ] / 10 ) - magic_offset ,)
876
+ return (unpack ("<h" , msg .payload )[0 ] / 10 ) - magic_offset ,
866
877
867
878
@property
868
879
def temperature (self ):
0 commit comments