@@ -63,6 +63,7 @@ def begin(self) -> int:
63
63
sleep_ms (100 )
64
64
self ._reset_hw ()
65
65
sleep_ms (1000 )
66
+ self .set_illuminator (True )
66
67
return 0
67
68
68
69
def _begin_update_thread (self ):
@@ -285,6 +286,9 @@ def _parse_message(self) -> int:
285
286
elif code == ord ('q' ):
286
287
# imu position
287
288
_ , self .roll , self .pitch , self .yaw = self .packeter .unpacketC3F ()
289
+ if code == ord ('w' ):
290
+ # wheels position
291
+ _ , self .left_wheel ._position , self .right_wheel ._position = self .packeter .unpacketC2F ()
288
292
elif code == 0x7E :
289
293
# firmware version
290
294
_ , * self .version = self .packeter .unpacketC3B ()
@@ -399,14 +403,16 @@ def __init__(self, packeter: ucPack, label: int, wheel_diameter_mm: float = WHEE
399
403
self ._label = label
400
404
self ._wheel_diameter_mm = wheel_diameter_mm
401
405
self ._speed = None
406
+ self ._position = None
402
407
403
408
def reset (self , initial_position : float = 0.0 ):
404
409
"""
405
410
Resets the wheel reference position
406
411
:param initial_position:
407
412
:return:
408
413
"""
409
- pass
414
+ self ._packeter .packetC2B1F (ord ('W' ), self ._label & 0xFF , ord ('Z' ), initial_position )
415
+ uart .write (self ._packeter .msg [0 :self ._packeter .msg_size ])
410
416
411
417
def set_pid_gains (self , kp : float = MOTOR_KP_DEFAULT , ki : float = MOTOR_KI_DEFAULT , kd : float = MOTOR_KD_DEFAULT ):
412
418
"""
@@ -417,7 +423,7 @@ def set_pid_gains(self, kp: float = MOTOR_KP_DEFAULT, ki: float = MOTOR_KI_DEFAU
417
423
:return:
418
424
"""
419
425
420
- self ._packeter .packetC1B3F (ord ('P' ), self ._label , kp , ki , kd )
426
+ self ._packeter .packetC1B3F (ord ('P' ), self ._label & 0xFF , kp , ki , kd )
421
427
uart .write (self ._packeter .msg [0 :self ._packeter .msg_size ])
422
428
423
429
def stop (self ):
@@ -450,6 +456,13 @@ def get_speed(self) -> float:
450
456
"""
451
457
return self ._speed
452
458
459
+ def get_position (self ) -> float :
460
+ """
461
+ Returns the wheel position (angle with respect to the reference)
462
+ :return:
463
+ """
464
+ return self ._position
465
+
453
466
def set_position (self , position : float , unit : str = 'deg' ):
454
467
"""
455
468
Sets left/right motor speed
0 commit comments