From 76c831c5ef70dba5801af88de86082011981557f Mon Sep 17 00:00:00 2001 From: Lucio Rossi Date: Thu, 13 Mar 2025 17:53:29 +0100 Subject: [PATCH 1/4] feat: uart messaging r/w locks fix: fixes skipping host->robot commands on occasions --- arduino_alvik/arduino_alvik.py | 60 +++++++++++++++++++++++++++++----- 1 file changed, 51 insertions(+), 9 deletions(-) diff --git a/arduino_alvik/arduino_alvik.py b/arduino_alvik/arduino_alvik.py index 5330e5a..2b23459 100644 --- a/arduino_alvik/arduino_alvik.py +++ b/arduino_alvik/arduino_alvik.py @@ -16,12 +16,31 @@ from .__init__ import __required_firmware_version__ +def writes_uart(method): + def wrapper(*args, **kwargs): + with ArduinoAlvik._write_lock: + method(*args, **kwargs) + + return wrapper + + +def reads_uart(method): + def wrapper(*args, **kwargs): + with ArduinoAlvik._read_lock: + method(*args, **kwargs) + + return wrapper + + class ArduinoAlvik: _update_thread_running = False _update_thread_id = None _events_thread_running = False _events_thread_id = None + _write_lock = _thread.allocate_lock() + _read_lock = _thread.allocate_lock() + def __new__(cls): if not hasattr(cls, '_instance'): cls._instance = super(ArduinoAlvik, cls).__new__(cls) @@ -119,7 +138,8 @@ def _print_battery_status(percentage: float, is_charging) -> None: word = marks_str + f" {percentage}% {charging_str} \t" print(word, end='') - def _lenghty_op(self, iterations=10000000) -> int: + @staticmethod + def _lengthy_op(self, iterations=10000000) -> int: result = 0 for i in range(1, iterations): result += i * i @@ -134,7 +154,7 @@ def _idle(self, delay_=1, check_on_thread=False, blocking=False) -> None: self.i2c.set_single_thread(True) if blocking: - self._lenghty_op(50000) + self._lengthy_op(50000) else: sleep_ms(500) led_val = 0 @@ -157,7 +177,7 @@ def _idle(self, delay_=1, check_on_thread=False, blocking=False) -> None: self._battery_perc = abs(soc_perc) self._print_battery_status(round(soc_perc), self._battery_is_charging) if blocking: - self._lenghty_op(10000) + self._lengthy_op(10000) else: sleep_ms(delay_) if soc_perc > 97: @@ -279,6 +299,7 @@ def _wait_for_fw_check(self, timeout=5) -> bool: return False @staticmethod + @reads_uart def _flush_uart(): """ Empties the UART buffer @@ -313,6 +334,7 @@ def _wait_for_target(self, idle_time): # print(self._last_ack) sleep_ms(100) + @writes_uart def is_target_reached(self) -> bool: """ Returns True if robot has sent an M or R acknowledgment. @@ -330,6 +352,7 @@ def is_target_reached(self) -> bool: return True return False + @writes_uart def set_behaviour(self, behaviour: int): """ Sets the behaviour of Alvik @@ -339,6 +362,7 @@ def set_behaviour(self, behaviour: int): self._packeter.packetC1B(ord('B'), behaviour & 0xFF) uart.write(self._packeter.msg[0:self._packeter.msg_size]) + @writes_uart def rotate(self, angle: float, unit: str = 'deg', blocking: bool = True): """ Rotates the robot by given angle @@ -355,6 +379,7 @@ def rotate(self, angle: float, unit: str = 'deg', blocking: bool = True): if blocking: self._wait_for_target(idle_time=(angle / MOTOR_CONTROL_DEG_S)) + @writes_uart def move(self, distance: float, unit: str = 'cm', blocking: bool = True): """ Moves the robot by given distance @@ -408,6 +433,7 @@ def get_wheels_speed(self, unit: str = 'rpm') -> (float | None, float | None): """ return self.left_wheel.get_speed(unit), self.right_wheel.get_speed(unit) + @writes_uart def set_wheels_speed(self, left_speed: float, right_speed: float, unit: str = 'rpm'): """ Sets left/right motor speed @@ -427,6 +453,7 @@ def set_wheels_speed(self, left_speed: float, right_speed: float, unit: str = 'r self._packeter.packetC2F(ord('J'), left_speed, right_speed) uart.write(self._packeter.msg[0:self._packeter.msg_size]) + @writes_uart def set_wheels_position(self, left_angle: float, right_angle: float, unit: str = 'deg', blocking: bool = True): """ Sets left/right motor angle @@ -490,6 +517,7 @@ def get_line_sensors(self) -> (int | None, int | None, int | None): return self._left_line, self._center_line, self._right_line + @writes_uart def drive(self, linear_velocity: float, angular_velocity: float, linear_unit: str = 'cm/s', angular_unit: str = 'deg/s'): """ @@ -530,6 +558,7 @@ def get_drive_speed(self, linear_unit: str = 'cm/s', angular_unit: str = 'deg/s' return convert_speed(self._linear_velocity, 'mm/s', linear_unit), angular_velocity + @writes_uart def reset_pose(self, x: float, y: float, theta: float, distance_unit: str = 'cm', angle_unit: str = 'deg'): """ Resets the robot pose @@ -559,6 +588,7 @@ def get_pose(self, distance_unit: str = 'cm', angle_unit: str = 'deg') \ convert_distance(self._y, 'mm', distance_unit), convert_angle(self._theta, 'deg', angle_unit)) + @writes_uart def set_servo_positions(self, a_position: int, b_position: int): """ Sets A/B servomotor angle @@ -586,10 +616,16 @@ def get_ack(self) -> str: """ return self._last_ack - # def send_ack(self): - # self._packeter.packetC1B(ord('X'), ACK_) - # uart.write(self._packeter.msg[0:self._packeter.msg_size]) + @writes_uart + def send_ack(self, ack: str = 'K'): + """ + Sends an ack message on UART + :return: + """ + self._packeter.packetC1B(ord('X'), ord(ack)) + uart.write(self._packeter.msg[0:self._packeter.msg_size]) + @writes_uart def _set_leds(self, led_state: int): """ Sets the LEDs state @@ -651,6 +687,7 @@ def _update(self, delay_=1): self._read_message() sleep_ms(delay_) + @reads_uart def _read_message(self) -> None: """ Read a message from the uC @@ -1539,7 +1576,6 @@ def writeto_mem(self, addr, memaddr, buf, addrsize=8) -> None: return i2c.writeto_mem(addr, memaddr, buf, addrsize=addrsize) - class _ArduinoAlvikServo: def __init__(self, packeter: ucPack, label: str, servo_id: int, position: list[int | None]): @@ -1547,7 +1583,8 @@ def __init__(self, packeter: ucPack, label: str, servo_id: int, position: list[i self._label = label self._id = servo_id self._position = position - + + @writes_uart def set_position(self, position): """ Sets the position of the servo @@ -1576,7 +1613,8 @@ def __init__(self, packeter: ucPack, label: int, wheel_diameter_mm: float = WHEE self._speed = None self._position = None self._alvik = alvik - + + @writes_uart def reset(self, initial_position: float = 0.0, unit: str = 'deg'): """ Resets the wheel reference position @@ -1588,6 +1626,7 @@ def reset(self, initial_position: float = 0.0, unit: str = 'deg'): self._packeter.packetC2B1F(ord('W'), self._label & 0xFF, ord('Z'), initial_position) uart.write(self._packeter.msg[0:self._packeter.msg_size]) + @writes_uart def set_pid_gains(self, kp: float = MOTOR_KP_DEFAULT, ki: float = MOTOR_KI_DEFAULT, kd: float = MOTOR_KD_DEFAULT): """ Set PID gains for Alvik wheels @@ -1607,6 +1646,7 @@ def stop(self): """ self.set_speed(0) + @writes_uart def set_speed(self, velocity: float, unit: str = 'rpm'): """ Sets the motor speed @@ -1643,6 +1683,7 @@ def get_position(self, unit: str = 'deg') -> float | None: """ return convert_angle(self._position, 'deg', unit) + @writes_uart def set_position(self, position: float, unit: str = 'deg', blocking: bool = True): """ Sets left/right motor speed @@ -1673,6 +1714,7 @@ def __init__(self, packeter: ucPack, label: str, led_state: list[int | None], rg self._rgb_mask = rgb_mask self._led_state = led_state + @writes_uart def set_color(self, red: bool, green: bool, blue: bool): """ Sets the LED's r,g,b state From ea12960641c5eede3397f5e58afc9c0ae857bac3 Mon Sep 17 00:00:00 2001 From: Lucio Rossi Date: Tue, 18 Mar 2025 12:50:47 +0100 Subject: [PATCH 2/4] fix: read/write uart decorators not returning method output --- arduino_alvik/arduino_alvik.py | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/arduino_alvik/arduino_alvik.py b/arduino_alvik/arduino_alvik.py index 2b23459..239fd6d 100644 --- a/arduino_alvik/arduino_alvik.py +++ b/arduino_alvik/arduino_alvik.py @@ -19,7 +19,7 @@ def writes_uart(method): def wrapper(*args, **kwargs): with ArduinoAlvik._write_lock: - method(*args, **kwargs) + return method(*args, **kwargs) return wrapper @@ -27,7 +27,7 @@ def wrapper(*args, **kwargs): def reads_uart(method): def wrapper(*args, **kwargs): with ArduinoAlvik._read_lock: - method(*args, **kwargs) + return method(*args, **kwargs) return wrapper From b55eb259323df556c483d4bc6bf4d8bc17371a8c Mon Sep 17 00:00:00 2001 From: Lucio Rossi Date: Tue, 18 Mar 2025 16:43:12 +0100 Subject: [PATCH 3/4] fix: read/write uart decorators can deadlock feat: introduce _AlvikRLock alvik's read/write uart re-entrant lock --- arduino_alvik/arduino_alvik.py | 51 ++++++++++++++++++++++++++++++---- 1 file changed, 46 insertions(+), 5 deletions(-) diff --git a/arduino_alvik/arduino_alvik.py b/arduino_alvik/arduino_alvik.py index 239fd6d..54b1d73 100644 --- a/arduino_alvik/arduino_alvik.py +++ b/arduino_alvik/arduino_alvik.py @@ -32,14 +32,55 @@ def wrapper(*args, **kwargs): return wrapper +class _AlvikRLock: + def __init__(self): + """Alvik re-entrant Lock implementation""" + self._lock = _thread.allocate_lock() + self._owner = None + self._count = 0 + + def acquire(self): + tid = _thread.get_ident() + + if self._owner == tid: + self._count += 1 + return True + + self._lock.acquire() + self._owner = tid + self._count = 1 + return True + + def release(self): + tid = _thread.get_ident() + + if self._owner != tid: + raise RuntimeError("Cannot release an unowned lock") + + self._count -= 1 + if self._count == 0: + self._owner = None + self._lock.release() + + def locked(self): + return self._lock.locked() + + def __enter__(self): + self.acquire() + return self + + def __exit__(self, exc_type, exc_value, traceback): + self.release() + + class ArduinoAlvik: _update_thread_running = False _update_thread_id = None _events_thread_running = False _events_thread_id = None - _write_lock = _thread.allocate_lock() - _read_lock = _thread.allocate_lock() + _write_lock = _AlvikRLock() + _read_lock = _AlvikRLock() def __new__(cls): if not hasattr(cls, '_instance'): @@ -189,13 +230,13 @@ def _idle(self, delay_=1, check_on_thread=False, blocking=False) -> None: led_val = (led_val + 1) % 2 self.i2c.set_single_thread(False) if self.is_on(): - print("********** Alvik is on **********") + print("\n********** Alvik is on **********") except KeyboardInterrupt: self.stop() sys.exit() except Exception as e: pass - print(f'Unable to read SOC: {e}') + print(f'\nUnable to read SOC: {e}') finally: LEDR.value(1) LEDG.value(1) @@ -252,7 +293,7 @@ def begin(self) -> int: self.set_behaviour(2) self._set_color_reference() if self._has_events_registered(): - print('Starting events thread') + print('\n********** Starting events thread **********\n') self._start_events_thread() self.set_servo_positions(90, 90) return 0 From c08e38a4a6c8a9748f69e4a4b273b0225eeafaa5 Mon Sep 17 00:00:00 2001 From: Lucio Rossi Date: Wed, 19 Mar 2025 16:02:36 +0100 Subject: [PATCH 4/4] ver: 1.1.3 ready update required FW version to 1.1.1 --- arduino_alvik/__init__.py | 4 ++-- package.json | 2 +- 2 files changed, 3 insertions(+), 3 deletions(-) diff --git a/arduino_alvik/__init__.py b/arduino_alvik/__init__.py index dbdfc6a..00a67aa 100644 --- a/arduino_alvik/__init__.py +++ b/arduino_alvik/__init__.py @@ -4,8 +4,8 @@ __author__ = "Lucio Rossi , Giovanni Bruno " __license__ = "MPL 2.0" -__version__ = "1.1.2" +__version__ = "1.1.3" __maintainer__ = "Lucio Rossi , Giovanni Bruno " -__required_firmware_version__ = "1.1.0" +__required_firmware_version__ = "1.1.1" from .arduino_alvik import * diff --git a/package.json b/package.json index 134ac1b..f946578 100644 --- a/package.json +++ b/package.json @@ -13,5 +13,5 @@ ["github:arduino/ucPack-mpy", "0.1.7"], ["github:arduino/arduino-runtime-mpy", "0.4.0"] ], - "version": "1.1.2" + "version": "1.1.3" }