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/arduino_alvik/arduino_alvik.py b/arduino_alvik/arduino_alvik.py index 5330e5a..54b1d73 100644 --- a/arduino_alvik/arduino_alvik.py +++ b/arduino_alvik/arduino_alvik.py @@ -16,12 +16,72 @@ from .__init__ import __required_firmware_version__ +def writes_uart(method): + def wrapper(*args, **kwargs): + with ArduinoAlvik._write_lock: + return method(*args, **kwargs) + + return wrapper + + +def reads_uart(method): + def wrapper(*args, **kwargs): + with ArduinoAlvik._read_lock: + return method(*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 = _AlvikRLock() + _read_lock = _AlvikRLock() + def __new__(cls): if not hasattr(cls, '_instance'): cls._instance = super(ArduinoAlvik, cls).__new__(cls) @@ -119,7 +179,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 +195,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 +218,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: @@ -169,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) @@ -232,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 @@ -279,6 +340,7 @@ def _wait_for_fw_check(self, timeout=5) -> bool: return False @staticmethod + @reads_uart def _flush_uart(): """ Empties the UART buffer @@ -313,6 +375,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 +393,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 +403,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 +420,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 +474,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 +494,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 +558,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 +599,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 +629,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 +657,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 +728,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 +1617,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 +1624,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 +1654,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 +1667,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 +1687,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 +1724,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 +1755,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 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" }