From 7d3aa5a9f3f934a84d88dd3f244b3cf481be1af4 Mon Sep 17 00:00:00 2001 From: Lucio Rossi Date: Mon, 19 Feb 2024 11:12:40 +0100 Subject: [PATCH 01/25] impr: documentation --- arduino_alvik.py | 20 +++++++++++++++++--- 1 file changed, 17 insertions(+), 3 deletions(-) diff --git a/arduino_alvik.py b/arduino_alvik.py index 9161470..3f4edcc 100644 --- a/arduino_alvik.py +++ b/arduino_alvik.py @@ -111,6 +111,11 @@ def _wait_for_target(self): pass def is_target_reached(self) -> bool: + """ + Returns True if robot has sent an M or R acknowledgment. + It also responds with an ack received message + :return: + """ if self.last_ack != ord('M') and self.last_ack != ord('R'): sleep_ms(50) return False @@ -192,6 +197,7 @@ def _reset_hw(): def get_wheels_speed(self, unit: str = 'rpm') -> (float, float): """ Returns the speed of the wheels + :param unit: the speed unit of measurement (default: 'rpm') :return: left_wheel_speed, right_wheel_speed """ return self.left_wheel.get_speed(unit), self.right_wheel.get_speed(unit) @@ -230,6 +236,7 @@ def set_wheels_position(self, left_angle: float, right_angle: float, unit: str = def get_wheels_position(self, unit: str = 'deg') -> (float, float): """ Returns the angle of the wheels + :param unit: the angle unit of measurement (default: 'deg') :return: left_wheel_angle, right_wheel_angle """ return (convert_angle(self.left_wheel.get_position(), 'deg', unit), @@ -351,7 +358,7 @@ def set_servo_positions(self, a_position: int, b_position: int): def get_ack(self): """ - Resets and returns last acknowledgement + Returns last acknowledgement :return: """ return self.last_ack @@ -601,7 +608,7 @@ def color_calibration(self, background: str = 'white') -> None: try: with open(file_path, 'r') as file: content = file.read().split('\n') - lines = [l + '\n' for l in content if l] + lines = [line + '\n' for line in content if line] except OSError: open(file_path, 'a').close() lines = [] @@ -699,9 +706,16 @@ def get_color(self, color_format: str = 'rgb') -> (float, float, float): return self._normalize_color(*self.get_color_raw()) elif color_format == 'hsv': return self.rgb2hsv(*self._normalize_color(*self.get_color_raw())) + @staticmethod def get_color_label(h, s, v) -> str: - label = 'UNDEFINED' + """ + Returns the color label corresponding to the given normalized HSV color input + :param h: + :param s: + :param v: + :return: + """ if s < 0.1: if v < 0.05: From a930f819663f39058efacf9a84ece0319618f551 Mon Sep 17 00:00:00 2001 From: Lucio Rossi Date: Mon, 19 Feb 2024 11:57:36 +0100 Subject: [PATCH 02/25] fix: typo --- pinout_definitions.py | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/pinout_definitions.py b/pinout_definitions.py index 5551eb1..feb7ab3 100644 --- a/pinout_definitions.py +++ b/pinout_definitions.py @@ -11,7 +11,7 @@ BOOT0_STM32 = Pin(D2, Pin.OUT) # nano D2 -> STM32 Boot0 RESET_STM32 = Pin(D3, Pin.OUT) # nano D3 -> STM32 NRST -NANO_CHK = Pin(D4, Pin.OUT) # nano D3 -> STM32 NANO_CHK +NANO_CHK = Pin(D4, Pin.OUT) # nano D4 -> STM32 NANO_CHK CHECK_STM32 = Pin(A6, Pin.IN, Pin.PULL_UP) # nano A6/D23 -> STM32 ROBOT_CHK ESP32_SDA = Pin(A4, Pin.OUT) # ESP32_SDA ESP32_SCL = Pin(A5, Pin.OUT) # ESP32_SCL From 19bd343a66ffbb793e989804e46bb1580fd77ff0 Mon Sep 17 00:00:00 2001 From: Lucio Rossi Date: Mon, 19 Feb 2024 12:09:49 +0100 Subject: [PATCH 03/25] fix: message_reader not setting initial speed --- examples/message_reader.py | 2 ++ 1 file changed, 2 insertions(+) diff --git a/examples/message_reader.py b/examples/message_reader.py index 57b3dd0..9c0a582 100644 --- a/examples/message_reader.py +++ b/examples/message_reader.py @@ -6,6 +6,8 @@ if alvik.begin() < 0: sys.exit() +speed = 0 + while True: try: print(f'VER: {alvik.version}') From 3c14a853440fbc2107a02150aadc3458fc2bd7d4 Mon Sep 17 00:00:00 2001 From: Lucio Rossi Date: Mon, 19 Feb 2024 14:39:35 +0100 Subject: [PATCH 04/25] feat: idle state --- arduino_alvik.py | 76 +++++++++++++++++++++++++++++++++++++++---- examples/test_idle.py | 23 +++++++++++++ 2 files changed, 93 insertions(+), 6 deletions(-) create mode 100644 examples/test_idle.py diff --git a/arduino_alvik.py b/arduino_alvik.py index 3f4edcc..0fcbccc 100644 --- a/arduino_alvik.py +++ b/arduino_alvik.py @@ -1,5 +1,8 @@ +import sys import gc +import struct +from machine import I2C from uart import uart import _thread from time import sleep_ms @@ -66,28 +69,86 @@ def __init__(self): self.last_ack = '' self.version = [None, None, None] + @staticmethod + def is_alvik_on() -> bool: + """ + Returns true if robot is on + :return: + """ + return CHECK_STM32.value() == 1 + + def _idle(self, delay_=1) -> None: + """ + Alvik's idle mode behaviour + :return: + """ + + NANO_CHK.value(1) + sleep_ms(500) + + while not self.is_alvik_on(): + try: + ESP32_SDA = Pin(A4, Pin.OUT) + ESP32_SCL = Pin(A5, Pin.OUT) + ESP32_SCL.value(1) + ESP32_SDA.value(1) + sleep_ms(100) + ESP32_SCL.value(0) + ESP32_SDA.value(0) + + cmd = bytearray(1) + cmd[0] = 0x06 + i2c = I2C(0, scl=ESP32_SCL, sda=ESP32_SDA) + i2c.writeto(0x36, cmd) + soc_raw = struct.unpack('h', i2c.readfrom(0x36, 2))[0] + print(f"SOC % : {soc_raw*0.00390625}") + sleep_ms(delay_) + except KeyboardInterrupt: + self.stop() + sys.exit() + except Exception as e: + print(f'Unable to read SOC: {e}') + + NANO_CHK.value(0) + def begin(self) -> int: """ Begins all Alvik operations :return: """ - if not CHECK_STM32.value(): + if not self.is_alvik_on(): print("\nTurn on your Arduino Alvik!\n") - return -1 + sleep_ms(1000) + self._idle() self._begin_update_thread() sleep_ms(100) self._reset_hw() - while uart.any(): - uart.read(1) + self._flush_uart() sleep_ms(1000) - while self.last_ack != 0x00: - sleep_ms(20) + self._wait_for_ack() sleep_ms(2000) self.set_illuminator(True) self.set_behaviour(1) self._set_color_reference() return 0 + def _wait_for_ack(self) -> None: + """ + Waits until receives 0x00 ack from robot + :return: + """ + while self.last_ack != 0x00: + sleep_ms(20) + + @staticmethod + def _flush_uart(): + """ + Empties the UART buffer + :return: + """ + while uart.any(): + uart.read(1) + def _begin_update_thread(self): """ Runs robot background operations (e.g. threaded update) @@ -409,6 +470,9 @@ def _update(self, delay_=1): :return: """ while True: + if not self.is_alvik_on(): + print("Alvik not connected") + self._idle(delay_) if not ArduinoAlvik._update_thread_running: break if self._read_message(): diff --git a/examples/test_idle.py b/examples/test_idle.py new file mode 100644 index 0000000..cf2d761 --- /dev/null +++ b/examples/test_idle.py @@ -0,0 +1,23 @@ +from arduino_alvik import ArduinoAlvik +from time import sleep_ms +import sys + +alvik = ArduinoAlvik() +alvik.begin() + +speed = 0 + +while True: + try: + + if alvik.is_alvik_on(): + print(f'VER: {alvik.version}') + print(f'LSP: {alvik.left_wheel.get_speed()}') + alvik.set_wheels_speed(speed, speed) + speed = (speed + 1) % 30 + sleep_ms(1000) + except KeyboardInterrupt as e: + print('over') + alvik.stop() + sys.exit() + From 1e22b5fd00658d767fca497ced214daa2421fe24 Mon Sep 17 00:00:00 2001 From: Lucio Rossi Date: Mon, 19 Feb 2024 17:16:57 +0100 Subject: [PATCH 05/25] fix: CHECK_STM32 must be PULL_DOWN --- pinout_definitions.py | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/pinout_definitions.py b/pinout_definitions.py index feb7ab3..efc2a54 100644 --- a/pinout_definitions.py +++ b/pinout_definitions.py @@ -12,6 +12,6 @@ BOOT0_STM32 = Pin(D2, Pin.OUT) # nano D2 -> STM32 Boot0 RESET_STM32 = Pin(D3, Pin.OUT) # nano D3 -> STM32 NRST NANO_CHK = Pin(D4, Pin.OUT) # nano D4 -> STM32 NANO_CHK -CHECK_STM32 = Pin(A6, Pin.IN, Pin.PULL_UP) # nano A6/D23 -> STM32 ROBOT_CHK +CHECK_STM32 = Pin(A6, Pin.IN, Pin.PULL_DOWN) # nano A6/D23 -> STM32 ROBOT_CHK ESP32_SDA = Pin(A4, Pin.OUT) # ESP32_SDA ESP32_SCL = Pin(A5, Pin.OUT) # ESP32_SCL From a1c94a47efe138740bb19c614d7744d06c5ec086 Mon Sep 17 00:00:00 2001 From: Lucio Rossi Date: Mon, 19 Feb 2024 17:41:36 +0100 Subject: [PATCH 06/25] feat: SOC with leds, _upadate resets STM32 after idle --- arduino_alvik.py | 21 +++++++++++++++++++-- pinout_definitions.py | 5 +++++ 2 files changed, 24 insertions(+), 2 deletions(-) diff --git a/arduino_alvik.py b/arduino_alvik.py index 0fcbccc..ef5308e 100644 --- a/arduino_alvik.py +++ b/arduino_alvik.py @@ -85,7 +85,7 @@ def _idle(self, delay_=1) -> None: NANO_CHK.value(1) sleep_ms(500) - + led_val = 0 while not self.is_alvik_on(): try: ESP32_SDA = Pin(A4, Pin.OUT) @@ -101,14 +101,24 @@ def _idle(self, delay_=1) -> None: i2c = I2C(0, scl=ESP32_SCL, sda=ESP32_SDA) i2c.writeto(0x36, cmd) soc_raw = struct.unpack('h', i2c.readfrom(0x36, 2))[0] - print(f"SOC % : {soc_raw*0.00390625}") + soc_perc = soc_raw*0.00390625 + print(f"SOC % : {round(soc_perc)}") sleep_ms(delay_) + if soc_perc > 98: + LEDG.value(0) + LEDR.value(1) + else: + LEDR.value(led_val) + LEDG.value(1) + led_val = (led_val + 1) % 2 except KeyboardInterrupt: self.stop() sys.exit() except Exception as e: print(f'Unable to read SOC: {e}') + LEDR.value(1) + LEDG.value(1) NANO_CHK.value(0) def begin(self) -> int: @@ -473,6 +483,13 @@ def _update(self, delay_=1): if not self.is_alvik_on(): print("Alvik not connected") self._idle(delay_) + self._reset_hw() + self._flush_uart() + sleep_ms(1000) + self._wait_for_ack() + sleep_ms(2000) + self.set_illuminator(True) + self.set_behaviour(1) if not ArduinoAlvik._update_thread_running: break if self._read_message(): diff --git a/pinout_definitions.py b/pinout_definitions.py index efc2a54..0a5b1c3 100644 --- a/pinout_definitions.py +++ b/pinout_definitions.py @@ -15,3 +15,8 @@ CHECK_STM32 = Pin(A6, Pin.IN, Pin.PULL_DOWN) # nano A6/D23 -> STM32 ROBOT_CHK ESP32_SDA = Pin(A4, Pin.OUT) # ESP32_SDA ESP32_SCL = Pin(A5, Pin.OUT) # ESP32_SCL + +# LEDS +LEDR = Pin(46, Pin.OUT) #RED ESP32 LEDR +LEDG = Pin(0, Pin.OUT) #GREEN ESP32 LEDG +LEDB = Pin(45, Pin.OUT) #BLUE ESP32 LEDB From 4e19bc75e964ea45c2943637ed23f570c6b7ae7b Mon Sep 17 00:00:00 2001 From: Lucio Rossi Date: Tue, 20 Feb 2024 10:14:53 +0100 Subject: [PATCH 07/25] feat: SOC _progress_bar on _idle --- arduino_alvik.py | 19 +++++++++++++++++-- 1 file changed, 17 insertions(+), 2 deletions(-) diff --git a/arduino_alvik.py b/arduino_alvik.py index ef5308e..b55c9a4 100644 --- a/arduino_alvik.py +++ b/arduino_alvik.py @@ -77,6 +77,19 @@ def is_alvik_on() -> bool: """ return CHECK_STM32.value() == 1 + @staticmethod + def _progress_bar(percentage: float) -> None: + """ + Prints a progressbar + :param percentage: + :return: + """ + sys.stdout.write('\r') + marks = int(percentage / 25) + #marks_str = '▏' + '▋'*marks + '░'*(4-marks) + '' + marks_str = '🔋' + sys.stdout.write(marks_str + f" {percentage}% \t") + def _idle(self, delay_=1) -> None: """ Alvik's idle mode behaviour @@ -102,7 +115,8 @@ def _idle(self, delay_=1) -> None: i2c.writeto(0x36, cmd) soc_raw = struct.unpack('h', i2c.readfrom(0x36, 2))[0] soc_perc = soc_raw*0.00390625 - print(f"SOC % : {round(soc_perc)}") + #print(f"SOC % : {round(soc_perc)}") + self._progress_bar(round(soc_perc)) sleep_ms(delay_) if soc_perc > 98: LEDG.value(0) @@ -115,7 +129,8 @@ def _idle(self, delay_=1) -> None: self.stop() sys.exit() except Exception as e: - print(f'Unable to read SOC: {e}') + pass + #print(f'Unable to read SOC: {e}') LEDR.value(1) LEDG.value(1) From 061731c5c3d72e251db093358e0f31a3713ce040 Mon Sep 17 00:00:00 2001 From: Lucio Rossi Date: Tue, 20 Feb 2024 11:33:47 +0100 Subject: [PATCH 08/25] feat: SOC progress bar with emojis --- arduino_alvik.py | 12 ++++++------ 1 file changed, 6 insertions(+), 6 deletions(-) diff --git a/arduino_alvik.py b/arduino_alvik.py index b55c9a4..f5b4732 100644 --- a/arduino_alvik.py +++ b/arduino_alvik.py @@ -85,9 +85,10 @@ def _progress_bar(percentage: float) -> None: :return: """ sys.stdout.write('\r') - marks = int(percentage / 25) - #marks_str = '▏' + '▋'*marks + '░'*(4-marks) + '' - marks_str = '🔋' + if percentage > 98: + marks_str = ' \U0001F50B' + else: + marks_str = ' \U0001FAAB' sys.stdout.write(marks_str + f" {percentage}% \t") def _idle(self, delay_=1) -> None: @@ -115,7 +116,6 @@ def _idle(self, delay_=1) -> None: i2c.writeto(0x36, cmd) soc_raw = struct.unpack('h', i2c.readfrom(0x36, 2))[0] soc_perc = soc_raw*0.00390625 - #print(f"SOC % : {round(soc_perc)}") self._progress_bar(round(soc_perc)) sleep_ms(delay_) if soc_perc > 98: @@ -435,8 +435,8 @@ def get_pose(self, distance_unit: str = 'cm', angle_unit: str = 'deg') -> (float def set_servo_positions(self, a_position: int, b_position: int): """ Sets A/B servomotor angle - :param a_position: position of A servomotor (0-180°) - :param b_position: position of B servomotor (0-180°) + :param a_position: position of A servomotor (0-180) + :param b_position: position of B servomotor (0-180) :return: """ self.packeter.packetC2B(ord('S'), a_position & 0xFF, b_position & 0xFF) From 9082cb4dfda2e4cdc3b1f8abea74a4b31547c3cf Mon Sep 17 00:00:00 2001 From: Lucio Rossi Date: Tue, 20 Feb 2024 14:23:29 +0100 Subject: [PATCH 09/25] feat: snake vs robot animation --- arduino_alvik.py | 29 +++++++++++++++++++++++++++-- 1 file changed, 27 insertions(+), 2 deletions(-) diff --git a/arduino_alvik.py b/arduino_alvik.py index f5b4732..90c2454 100644 --- a/arduino_alvik.py +++ b/arduino_alvik.py @@ -136,6 +136,31 @@ def _idle(self, delay_=1) -> None: LEDG.value(1) NANO_CHK.value(0) + def _snake_robot(self, duration: int = 1000): + """ + Snake robot animation + :param percentage: + :return: + """ + i = 0 + + robot = '\U0001F916' + snake = '\U0001F40D' + + cycles = int(duration / 200) + + for i in range(0, cycles): + sys.stdout.write('\r') + pre = ' ' * i + between = ' ' * (i % 2 + 1) + post = ' ' * 5 + frame = pre + snake + between + robot + post + sys.stdout.write(frame) + sleep_ms(200) + + sys.stdout.write('\r') + sys.stdout.write('') + def begin(self) -> int: """ Begins all Alvik operations @@ -149,9 +174,9 @@ def begin(self) -> int: sleep_ms(100) self._reset_hw() self._flush_uart() - sleep_ms(1000) + self._snake_robot(1000) self._wait_for_ack() - sleep_ms(2000) + self._snake_robot(2000) self.set_illuminator(True) self.set_behaviour(1) self._set_color_reference() From d1a1a3b09f9689ab5e46f6f37e60797b554f032f Mon Sep 17 00:00:00 2001 From: Lucio Rossi Date: Tue, 20 Feb 2024 17:38:26 +0100 Subject: [PATCH 10/25] fix: update thread stuck on idle --- arduino_alvik.py | 31 +++++++++++++++++-------------- 1 file changed, 17 insertions(+), 14 deletions(-) diff --git a/arduino_alvik.py b/arduino_alvik.py index 90c2454..24eed26 100644 --- a/arduino_alvik.py +++ b/arduino_alvik.py @@ -91,7 +91,7 @@ def _progress_bar(percentage: float) -> None: marks_str = ' \U0001FAAB' sys.stdout.write(marks_str + f" {percentage}% \t") - def _idle(self, delay_=1) -> None: + def _idle(self, delay_=1, check_on_thread=False) -> None: """ Alvik's idle mode behaviour :return: @@ -100,8 +100,11 @@ def _idle(self, delay_=1) -> None: NANO_CHK.value(1) sleep_ms(500) led_val = 0 - while not self.is_alvik_on(): - try: + + try: + while not self.is_alvik_on(): + if check_on_thread and not self.__class__._update_thread_running: + break ESP32_SDA = Pin(A4, Pin.OUT) ESP32_SCL = Pin(A5, Pin.OUT) ESP32_SCL.value(1) @@ -125,16 +128,16 @@ def _idle(self, delay_=1) -> None: LEDR.value(led_val) LEDG.value(1) led_val = (led_val + 1) % 2 - except KeyboardInterrupt: - self.stop() - sys.exit() - except Exception as e: - pass - #print(f'Unable to read SOC: {e}') - - LEDR.value(1) - LEDG.value(1) - NANO_CHK.value(0) + except KeyboardInterrupt: + self.stop() + sys.exit() + except Exception as e: + pass + #print(f'Unable to read SOC: {e}') + finally: + LEDR.value(1) + LEDG.value(1) + NANO_CHK.value(0) def _snake_robot(self, duration: int = 1000): """ @@ -522,7 +525,7 @@ def _update(self, delay_=1): while True: if not self.is_alvik_on(): print("Alvik not connected") - self._idle(delay_) + self._idle(delay_, check_on_thread=True) self._reset_hw() self._flush_uart() sleep_ms(1000) From 5c6a159e138270fafe5857cbde133b2766338a05 Mon Sep 17 00:00:00 2001 From: Lucio Rossi Date: Tue, 20 Feb 2024 18:04:34 +0100 Subject: [PATCH 11/25] impr: Alvik on/off repl output --- arduino_alvik.py | 3 ++- 1 file changed, 2 insertions(+), 1 deletion(-) diff --git a/arduino_alvik.py b/arduino_alvik.py index 24eed26..e907c97 100644 --- a/arduino_alvik.py +++ b/arduino_alvik.py @@ -128,6 +128,7 @@ def _idle(self, delay_=1, check_on_thread=False) -> None: LEDR.value(led_val) LEDG.value(1) led_val = (led_val + 1) % 2 + print("Alvik is on") except KeyboardInterrupt: self.stop() sys.exit() @@ -524,7 +525,7 @@ def _update(self, delay_=1): """ while True: if not self.is_alvik_on(): - print("Alvik not connected") + print("Alvik is off") self._idle(delay_, check_on_thread=True) self._reset_hw() self._flush_uart() From 2709b878a07a6afe461184871b058615301428c5 Mon Sep 17 00:00:00 2001 From: Lucio Rossi Date: Wed, 21 Feb 2024 12:08:19 +0100 Subject: [PATCH 12/25] fix: checking VIOLET label 2 times --- arduino_alvik.py | 2 -- 1 file changed, 2 deletions(-) diff --git a/arduino_alvik.py b/arduino_alvik.py index e907c97..ec1b076 100644 --- a/arduino_alvik.py +++ b/arduino_alvik.py @@ -865,8 +865,6 @@ def get_color_label(h, s, v) -> str: label = 'BLUE' elif 260 <= h < 280: label = 'VIOLET' - elif 260 <= h < 280: - label = 'VIOLET' else: # h<20 or h>=280 is more problematic if v < 0.5 and s < 0.45: label = 'BROWN' From 2bcb54ec02eba2c0459842fba142da755d20b726 Mon Sep 17 00:00:00 2001 From: Lucio Rossi Date: Fri, 23 Feb 2024 10:40:58 +0100 Subject: [PATCH 13/25] mod: replace is_alvik_on with is_on --- arduino_alvik.py | 8 ++++---- examples/test_idle.py | 2 +- 2 files changed, 5 insertions(+), 5 deletions(-) diff --git a/arduino_alvik.py b/arduino_alvik.py index ec1b076..8f6cbf0 100644 --- a/arduino_alvik.py +++ b/arduino_alvik.py @@ -70,7 +70,7 @@ def __init__(self): self.version = [None, None, None] @staticmethod - def is_alvik_on() -> bool: + def is_on() -> bool: """ Returns true if robot is on :return: @@ -102,7 +102,7 @@ def _idle(self, delay_=1, check_on_thread=False) -> None: led_val = 0 try: - while not self.is_alvik_on(): + while not self.is_on(): if check_on_thread and not self.__class__._update_thread_running: break ESP32_SDA = Pin(A4, Pin.OUT) @@ -170,7 +170,7 @@ def begin(self) -> int: Begins all Alvik operations :return: """ - if not self.is_alvik_on(): + if not self.is_on(): print("\nTurn on your Arduino Alvik!\n") sleep_ms(1000) self._idle() @@ -524,7 +524,7 @@ def _update(self, delay_=1): :return: """ while True: - if not self.is_alvik_on(): + if not self.is_on(): print("Alvik is off") self._idle(delay_, check_on_thread=True) self._reset_hw() diff --git a/examples/test_idle.py b/examples/test_idle.py index cf2d761..56659f2 100644 --- a/examples/test_idle.py +++ b/examples/test_idle.py @@ -10,7 +10,7 @@ while True: try: - if alvik.is_alvik_on(): + if alvik.is_on(): print(f'VER: {alvik.version}') print(f'LSP: {alvik.left_wheel.get_speed()}') alvik.set_wheels_speed(speed, speed) From c17c9a30c1f3ec9a8726c968d13e746f51ef1670 Mon Sep 17 00:00:00 2001 From: Lucio Rossi Date: Fri, 23 Feb 2024 14:31:36 +0100 Subject: [PATCH 14/25] fix: alvikdev-58 FW updater stuck if robot is off --- pinout_definitions.py | 2 +- utilities/firmware_updater.py | 7 ++++++- utilities/stm32_flash.py | 3 +++ 3 files changed, 10 insertions(+), 2 deletions(-) diff --git a/pinout_definitions.py b/pinout_definitions.py index 0a5b1c3..06d24a3 100644 --- a/pinout_definitions.py +++ b/pinout_definitions.py @@ -12,7 +12,7 @@ BOOT0_STM32 = Pin(D2, Pin.OUT) # nano D2 -> STM32 Boot0 RESET_STM32 = Pin(D3, Pin.OUT) # nano D3 -> STM32 NRST NANO_CHK = Pin(D4, Pin.OUT) # nano D4 -> STM32 NANO_CHK -CHECK_STM32 = Pin(A6, Pin.IN, Pin.PULL_DOWN) # nano A6/D23 -> STM32 ROBOT_CHK +CHECK_STM32 = Pin(A6, Pin.IN, Pin.PULL_DOWN) # nano A6/D23 -> STM32 ROBOT_CHK ESP32_SDA = Pin(A4, Pin.OUT) # ESP32_SDA ESP32_SCL = Pin(A5, Pin.OUT) # ESP32_SCL diff --git a/utilities/firmware_updater.py b/utilities/firmware_updater.py index 6a98ad2..0367704 100644 --- a/utilities/firmware_updater.py +++ b/utilities/firmware_updater.py @@ -1,9 +1,14 @@ from sys import exit from stm32_flash import * +if CHECK_STM32.value() is not 1: + print("Turn on your Alvik to continue...") + while CHECK_STM32.value() is not 1: + sleep_ms(500) + ans = STM32_startCommunication() if ans == STM32_NACK: - print("Cannot etablish connection with STM32") + print("Cannot establish connection with STM32") exit(-1) print('\nSTM32 FOUND') diff --git a/utilities/stm32_flash.py b/utilities/stm32_flash.py index 200af76..89522c6 100644 --- a/utilities/stm32_flash.py +++ b/utilities/stm32_flash.py @@ -3,6 +3,9 @@ from time import sleep_ms from machine import UART, Pin +A6 = 13 # ESP32 pin13 -> nano A6/D23 +CHECK_STM32 = Pin(A6, Pin.IN) # nano A6/D23 -> TO CHECK STM32 IS ON + STM32_INIT = b'\x7F' STM32_NACK = b'\x1F' STM32_ACK = b'\x79' From 69f4469ba5bc6a7b329902f9584caf12f62a11d0 Mon Sep 17 00:00:00 2001 From: Lucio Rossi Date: Fri, 23 Feb 2024 18:23:44 +0100 Subject: [PATCH 15/25] get_battery_charge --- arduino_alvik.py | 7 +++++++ examples/message_reader.py | 1 + 2 files changed, 8 insertions(+) diff --git a/arduino_alvik.py b/arduino_alvik.py index 8f6cbf0..0a8a90a 100644 --- a/arduino_alvik.py +++ b/arduino_alvik.py @@ -609,6 +609,13 @@ def _parse_message(self) -> int: return 0 + def get_battery_charge(self) -> float: + """ + Returns the battery SOC + :return: + """ + return self.battery_perc + def _get_touch(self) -> int: """ Returns the touch sensor's state diff --git a/examples/message_reader.py b/examples/message_reader.py index 9c0a582..c97db16 100644 --- a/examples/message_reader.py +++ b/examples/message_reader.py @@ -18,6 +18,7 @@ print(f'TOUCH: {alvik.touch_bits}') print(f'RGB: {alvik.red} {alvik.green} {alvik.blue}') print(f'LINE: {alvik.left_line} {alvik.center_line} {alvik.right_line}') + print(f'SOC: {alvik.get_battery_charge()}%') alvik.set_wheels_speed(speed, speed) speed = (speed + 1) % 60 From def43bab981b26a796b0ace9aa7b0b1ce64e0400 Mon Sep 17 00:00:00 2001 From: Lucio Rossi Date: Mon, 26 Feb 2024 09:42:46 +0100 Subject: [PATCH 16/25] mod: get_color_label split in hsv2label(h,s,v) and get_color_label() --- arduino_alvik.py | 9 ++++++++- examples/read_color_sensor.py | 2 +- 2 files changed, 9 insertions(+), 2 deletions(-) diff --git a/arduino_alvik.py b/arduino_alvik.py index 0a8a90a..3de887d 100644 --- a/arduino_alvik.py +++ b/arduino_alvik.py @@ -839,8 +839,15 @@ def get_color(self, color_format: str = 'rgb') -> (float, float, float): elif color_format == 'hsv': return self.rgb2hsv(*self._normalize_color(*self.get_color_raw())) + def get_color_label(self) -> str: + """ + Returns the label of the color as recognized by the sensor + :return: + """ + return self.hsv2label(*self.get_color(color_format='hsv')) + @staticmethod - def get_color_label(h, s, v) -> str: + def hsv2label(h, s, v) -> str: """ Returns the color label corresponding to the given normalized HSV color input :param h: diff --git a/examples/read_color_sensor.py b/examples/read_color_sensor.py index 994dbca..462826c 100644 --- a/examples/read_color_sensor.py +++ b/examples/read_color_sensor.py @@ -10,7 +10,7 @@ r, g, b = alvik.get_color() h, s, v = alvik.get_color('hsv') print(f'RED: {r}, Green: {g}, Blue: {b}, HUE: {h}, SAT: {s}, VAL: {v}') - print(f'COLOR LABEL: {alvik.get_color_label(h, s, v)}') + print(f'COLOR LABEL: {alvik.get_color_label()}') sleep_ms(100) except KeyboardInterrupt as e: print('over') From ba3e0b5676ec622234d9e36553215b7d7cd7fdb2 Mon Sep 17 00:00:00 2001 From: Lucio Rossi Date: Mon, 26 Feb 2024 09:52:24 +0100 Subject: [PATCH 17/25] mod: get_battery_charge returns an int --- arduino_alvik.py | 6 ++++-- 1 file changed, 4 insertions(+), 2 deletions(-) diff --git a/arduino_alvik.py b/arduino_alvik.py index 3de887d..01c6fa6 100644 --- a/arduino_alvik.py +++ b/arduino_alvik.py @@ -609,12 +609,14 @@ def _parse_message(self) -> int: return 0 - def get_battery_charge(self) -> float: + def get_battery_charge(self) -> int: """ Returns the battery SOC :return: """ - return self.battery_perc + if self.battery_perc > 100: + return 100 + return round(self.battery_perc) def _get_touch(self) -> int: """ From 1149178a90ebbca8eae9b609b48d4481d187ccad Mon Sep 17 00:00:00 2001 From: Lucio Rossi Date: Mon, 26 Feb 2024 10:01:58 +0100 Subject: [PATCH 18/25] fix: examples must not access class parameters directly --- examples/message_reader.py | 2 +- examples/test_idle.py | 2 +- 2 files changed, 2 insertions(+), 2 deletions(-) diff --git a/examples/message_reader.py b/examples/message_reader.py index c97db16..9b07128 100644 --- a/examples/message_reader.py +++ b/examples/message_reader.py @@ -15,7 +15,7 @@ print(f'RSP: {alvik.right_wheel.get_speed()}') print(f'LPOS: {alvik.left_wheel.get_position()}') print(f'RPOS: {alvik.right_wheel.get_position()}') - print(f'TOUCH: {alvik.touch_bits}') + print(f'TOUCH (UP): {alvik.get_touch_up()}') print(f'RGB: {alvik.red} {alvik.green} {alvik.blue}') print(f'LINE: {alvik.left_line} {alvik.center_line} {alvik.right_line}') print(f'SOC: {alvik.get_battery_charge()}%') diff --git a/examples/test_idle.py b/examples/test_idle.py index 56659f2..f65625a 100644 --- a/examples/test_idle.py +++ b/examples/test_idle.py @@ -11,7 +11,7 @@ try: if alvik.is_on(): - print(f'VER: {alvik.version}') + print(f'VER: {alvik.get_version()}') print(f'LSP: {alvik.left_wheel.get_speed()}') alvik.set_wheels_speed(speed, speed) speed = (speed + 1) % 30 From 27cc769b7259755e7826d7c4e742fde3e7d725f3 Mon Sep 17 00:00:00 2001 From: Lucio Rossi Date: Mon, 26 Feb 2024 10:57:47 +0100 Subject: [PATCH 19/25] fix: examples must not access class parameters directly --- examples/message_reader.py | 6 +++--- 1 file changed, 3 insertions(+), 3 deletions(-) diff --git a/examples/message_reader.py b/examples/message_reader.py index 9b07128..753dd68 100644 --- a/examples/message_reader.py +++ b/examples/message_reader.py @@ -10,14 +10,14 @@ while True: try: - print(f'VER: {alvik.version}') + print(f'VER: {alvik.get_version()}') print(f'LSP: {alvik.left_wheel.get_speed()}') print(f'RSP: {alvik.right_wheel.get_speed()}') print(f'LPOS: {alvik.left_wheel.get_position()}') print(f'RPOS: {alvik.right_wheel.get_position()}') print(f'TOUCH (UP): {alvik.get_touch_up()}') - print(f'RGB: {alvik.red} {alvik.green} {alvik.blue}') - print(f'LINE: {alvik.left_line} {alvik.center_line} {alvik.right_line}') + print(f'RGB: {alvik.get_color()}') + print(f'LINE: {alvik.get_line_sensors()}') print(f'SOC: {alvik.get_battery_charge()}%') alvik.set_wheels_speed(speed, speed) From 33a98f9a072f340855a6ae5b11f5c4c601003561 Mon Sep 17 00:00:00 2001 From: Lucio Rossi Date: Mon, 26 Feb 2024 12:33:55 +0100 Subject: [PATCH 20/25] feat: members as private --- arduino_alvik.py | 263 ++++++++++++++++++++++++----------------------- 1 file changed, 137 insertions(+), 126 deletions(-) diff --git a/arduino_alvik.py b/arduino_alvik.py index 01c6fa6..4705a96 100644 --- a/arduino_alvik.py +++ b/arduino_alvik.py @@ -21,53 +21,53 @@ class ArduinoAlvik: _update_thread_id = None def __new__(cls): - if not hasattr(cls, 'instance'): - cls.instance = super(ArduinoAlvik, cls).__new__(cls) - return cls.instance + if not hasattr(cls, '_instance'): + cls._instance = super(ArduinoAlvik, cls).__new__(cls) + return cls._instance def __init__(self): - self.packeter = ucPack(200) - self.left_wheel = _ArduinoAlvikWheel(self.packeter, ord('L')) - self.right_wheel = _ArduinoAlvikWheel(self.packeter, ord('R')) - self.led_state = [None] - self.left_led = _ArduinoAlvikRgbLed(self.packeter, 'left', self.led_state, + self._packeter = ucPack(200) + self.left_wheel = _ArduinoAlvikWheel(self._packeter, ord('L')) + self.right_wheel = _ArduinoAlvikWheel(self._packeter, ord('R')) + self._led_state = [None] + self.left_led = _ArduinoAlvikRgbLed(self._packeter, 'left', self._led_state, rgb_mask=[0b00000100, 0b00001000, 0b00010000]) - self.right_led = _ArduinoAlvikRgbLed(self.packeter, 'right', self.led_state, + self.right_led = _ArduinoAlvikRgbLed(self._packeter, 'right', self._led_state, rgb_mask=[0b00100000, 0b01000000, 0b10000000]) - self.battery_perc = None - self.touch_bits = None - self.behaviour = None - self.red = None - self.green = None - self.blue = None + self._battery_perc = None + self._touch_byte = None + self._behaviour = None + self._red = None + self._green = None + self._blue = None self._white_cal = None self._black_cal = None - self.left_line = None - self.center_line = None - self.right_line = None - self.roll = None - self.pitch = None - self.yaw = None - self.x = None - self.y = None - self.theta = None - self.ax = None - self.ay = None - self.az = None - self.gx = None - self.gy = None - self.gz = None - self.left_tof = None - self.center_left_tof = None - self.center_tof = None - self.center_right_tof = None - self.right_tof = None - self.top_tof = None - self.bottom_tof = None - self.linear_velocity = None - self.angular_velocity = None - self.last_ack = '' - self.version = [None, None, None] + self._left_line = None + self._center_line = None + self._right_line = None + self._roll = None + self._pitch = None + self._yaw = None + self._x = None + self._y = None + self._theta = None + self._ax = None + self._ay = None + self._az = None + self._gx = None + self._gy = None + self._gz = None + self._left_tof = None + self._center_left_tof = None + self._center_tof = None + self._center_right_tof = None + self._right_tof = None + self._top_tof = None + self._bottom_tof = None + self._linear_velocity = None + self._angular_velocity = None + self._angular_velocity = '' + self._version = [None, None, None] @staticmethod def is_on() -> bool: @@ -191,7 +191,7 @@ def _wait_for_ack(self) -> None: Waits until receives 0x00 ack from robot :return: """ - while self.last_ack != 0x00: + while self._angular_velocity != 0x00: sleep_ms(20) @staticmethod @@ -231,12 +231,12 @@ def is_target_reached(self) -> bool: It also responds with an ack received message :return: """ - if self.last_ack != ord('M') and self.last_ack != ord('R'): + if self._angular_velocity != ord('M') and self._angular_velocity != ord('R'): sleep_ms(50) return False else: - self.packeter.packetC1B(ord('X'), ord('K')) - uart.write(self.packeter.msg[0:self.packeter.msg_size]) + self._packeter.packetC1B(ord('X'), ord('K')) + uart.write(self._packeter.msg[0:self._packeter.msg_size]) sleep_ms(200) return True @@ -246,8 +246,8 @@ def set_behaviour(self, behaviour: int): :param behaviour: behaviour code :return: """ - self.packeter.packetC1B(ord('B'), behaviour & 0xFF) - uart.write(self.packeter.msg[0:self.packeter.msg_size]) + self._packeter.packetC1B(ord('B'), behaviour & 0xFF) + uart.write(self._packeter.msg[0:self._packeter.msg_size]) def rotate(self, angle: float, unit: str = 'deg', blocking: bool = True): """ @@ -259,8 +259,8 @@ def rotate(self, angle: float, unit: str = 'deg', blocking: bool = True): """ angle = convert_angle(angle, unit, 'deg') sleep_ms(200) - self.packeter.packetC1F(ord('R'), angle) - uart.write(self.packeter.msg[0:self.packeter.msg_size]) + self._packeter.packetC1F(ord('R'), angle) + uart.write(self._packeter.msg[0:self._packeter.msg_size]) if blocking: self._wait_for_target() @@ -274,8 +274,8 @@ def move(self, distance: float, unit: str = 'cm', blocking: bool = True): """ distance = convert_distance(distance, unit, 'mm') sleep_ms(200) - self.packeter.packetC1F(ord('G'), distance) - uart.write(self.packeter.msg[0:self.packeter.msg_size]) + self._packeter.packetC1F(ord('G'), distance) + uart.write(self._packeter.msg[0:self._packeter.msg_size]) if blocking: self._wait_for_target() @@ -293,8 +293,8 @@ def stop(self): # stop the update thrad self._stop_update_thread() - # delete instance - del self.__class__.instance + # delete _instance + del self.__class__._instance gc.collect() @staticmethod @@ -333,8 +333,8 @@ def set_wheels_speed(self, left_speed: float, right_speed: float, unit: str = 'r left_speed = convert_rotational_speed(left_speed, unit, 'rpm') right_speed = convert_rotational_speed(right_speed, unit, 'rpm') - self.packeter.packetC2F(ord('J'), left_speed, right_speed) - uart.write(self.packeter.msg[0:self.packeter.msg_size]) + self._packeter.packetC2F(ord('J'), left_speed, right_speed) + uart.write(self._packeter.msg[0:self._packeter.msg_size]) def set_wheels_position(self, left_angle: float, right_angle: float, unit: str = 'deg'): """ @@ -344,9 +344,9 @@ def set_wheels_position(self, left_angle: float, right_angle: float, unit: str = :param unit: the speed unit of measurement (default: 'rpm') :return: """ - self.packeter.packetC2F(ord('A'), convert_angle(left_angle, unit, 'deg'), + self._packeter.packetC2F(ord('A'), convert_angle(left_angle, unit, 'deg'), convert_angle(right_angle, unit, 'deg')) - uart.write(self.packeter.msg[0:self.packeter.msg_size]) + uart.write(self._packeter.msg[0:self._packeter.msg_size]) def get_wheels_position(self, unit: str = 'deg') -> (float, float): """ @@ -363,28 +363,28 @@ def get_orientation(self) -> (float, float, float): :return: roll, pitch, yaw """ - return self.roll, self.pitch, self.yaw + return self._roll, self._pitch, self._yaw def get_accelerations(self) -> (float, float, float): """ Returns the 3-axial acceleration of the IMU :return: ax, ay, az """ - return self.ax, self.ay, self.az + return self._ax, self._ay, self._az def get_gyros(self) -> (float, float, float): """ Returns the 3-axial angular acceleration of the IMU :return: gx, gy, gz """ - return self.gx, self.gy, self.gz + return self._gx, self._gy, self._gz def get_imu(self) -> (float, float, float, float, float, float): """ Returns all the IMUs readouts :return: ax, ay, az, gx, gy, gz """ - return self.ax, self.ay, self.az, self.gx, self.gy, self.gz + return self._ax, self._ay, self._az, self._gx, self._gy, self._gz def get_line_sensors(self) -> (int, int, int): """ @@ -392,7 +392,7 @@ def get_line_sensors(self) -> (int, int, int): :return: left_line, center_line, right_line """ - return self.left_line, self.center_line, self.right_line + return self._left_line, self._center_line, self._right_line def drive(self, linear_velocity: float, angular_velocity: float, linear_unit: str = 'cm/s', angular_unit: str = 'deg/s'): @@ -409,8 +409,8 @@ def drive(self, linear_velocity: float, angular_velocity: float, linear_unit: st angular_velocity = (angular_velocity/100)*ROBOT_MAX_DEG_S else: angular_velocity = convert_rotational_speed(angular_velocity, angular_unit, 'deg/s') - self.packeter.packetC2F(ord('V'), linear_velocity, angular_velocity) - uart.write(self.packeter.msg[0:self.packeter.msg_size]) + self._packeter.packetC2F(ord('V'), linear_velocity, angular_velocity) + uart.write(self._packeter.msg[0:self._packeter.msg_size]) def brake(self): """ @@ -427,11 +427,11 @@ def get_drive_speed(self, linear_unit: str = 'cm/s', angular_unit: str = 'deg/s' :return: linear_velocity, angular_velocity """ if angular_unit == '%': - angular_velocity = (self.angular_velocity/ROBOT_MAX_DEG_S)*100 + angular_velocity = (self._angular_velocity/ROBOT_MAX_DEG_S)*100 else: - angular_velocity = convert_rotational_speed(self.angular_velocity, 'deg/s', angular_unit) + angular_velocity = convert_rotational_speed(self._angular_velocity, 'deg/s', angular_unit) - return convert_speed(self.linear_velocity, 'mm/s', linear_unit), angular_velocity + return convert_speed(self._linear_velocity, 'mm/s', linear_unit), angular_velocity def reset_pose(self, x: float, y: float, theta: float, distance_unit: str = 'cm', angle_unit: str = 'deg'): """ @@ -446,8 +446,8 @@ def reset_pose(self, x: float, y: float, theta: float, distance_unit: str = 'cm' x = convert_distance(x, distance_unit, 'mm') y = convert_distance(y, distance_unit, 'mm') theta = convert_angle(theta, angle_unit, 'deg') - self.packeter.packetC3F(ord('Z'), x, y, theta) - uart.write(self.packeter.msg[0:self.packeter.msg_size]) + self._packeter.packetC3F(ord('Z'), x, y, theta) + uart.write(self._packeter.msg[0:self._packeter.msg_size]) sleep_ms(1000) def get_pose(self, distance_unit: str = 'cm', angle_unit: str = 'deg') -> (float, float, float): @@ -457,9 +457,9 @@ def get_pose(self, distance_unit: str = 'cm', angle_unit: str = 'deg') -> (float :param angle_unit: unit of theta output :return: x, y, theta """ - return (convert_distance(self.x, 'mm', distance_unit), - convert_distance(self.y, 'mm', distance_unit), - convert_angle(self.theta, 'deg', angle_unit)) + return (convert_distance(self._x, 'mm', distance_unit), + convert_distance(self._y, 'mm', distance_unit), + convert_angle(self._theta, 'deg', angle_unit)) def set_servo_positions(self, a_position: int, b_position: int): """ @@ -468,19 +468,19 @@ def set_servo_positions(self, a_position: int, b_position: int): :param b_position: position of B servomotor (0-180) :return: """ - self.packeter.packetC2B(ord('S'), a_position & 0xFF, b_position & 0xFF) - uart.write(self.packeter.msg[0:self.packeter.msg_size]) + self._packeter.packetC2B(ord('S'), a_position & 0xFF, b_position & 0xFF) + uart.write(self._packeter.msg[0:self._packeter.msg_size]) def get_ack(self): """ Returns last acknowledgement :return: """ - return self.last_ack + return self._angular_velocity # def send_ack(self): - # self.packeter.packetC1B(ord('X'), ACK_) - # uart.write(self.packeter.msg[0:self.packeter.msg_size]) + # self._packeter.packetC1B(ord('X'), ACK_) + # uart.write(self._packeter.msg[0:self._packeter.msg_size]) def _set_leds(self, led_state: int): """ @@ -489,9 +489,9 @@ def _set_leds(self, led_state: int): 5->right_red 6->right_green 7->right_blue :return: """ - self.led_state[0] = led_state & 0xFF - self.packeter.packetC1B(ord('L'), self.led_state[0]) - uart.write(self.packeter.msg[0:self.packeter.msg_size]) + self._led_state[0] = led_state & 0xFF + self._packeter.packetC1B(ord('L'), self._led_state[0]) + uart.write(self._packeter.msg[0:self._packeter.msg_size]) def set_builtin_led(self, value: bool): """ @@ -499,10 +499,10 @@ def set_builtin_led(self, value: bool): :param value: :return: """ - if self.led_state[0] is None: + if self._led_state[0] is None: self._set_leds(0x00) - self.led_state[0] = self.led_state[0] | 0b00000001 if value else self.led_state[0] & 0b11111110 - self._set_leds(self.led_state[0]) + self._led_state[0] = self._led_state[0] | 0b00000001 if value else self._led_state[0] & 0b11111110 + self._set_leds(self._led_state[0]) def set_illuminator(self, value: bool): """ @@ -510,10 +510,10 @@ def set_illuminator(self, value: bool): :param value: :return: """ - if self.led_state[0] is None: + if self._led_state[0] is None: self._set_leds(0x00) - self.led_state[0] = self.led_state[0] | 0b00000010 if value else self.led_state[0] & 0b11111101 - self._set_leds(self.led_state[0]) + self._led_state[0] = self._led_state[0] | 0b00000010 if value else self._led_state[0] & 0b11111101 + self._set_leds(self._led_state[0]) def _update(self, delay_=1): """ @@ -547,8 +547,8 @@ def _read_message(self) -> bool: """ while uart.any(): b = uart.read(1)[0] - self.packeter.buffer.push(b) - if b == self.packeter.end_index and self.packeter.checkPayload(): + self._packeter.buffer.push(b) + if b == self._packeter.end_index and self._packeter.checkPayload(): return True return False @@ -557,53 +557,53 @@ def _parse_message(self) -> int: Parse a received message :return: -1 if parse error 0 if ok """ - code = self.packeter.payloadTop() + code = self._packeter.payloadTop() if code == ord('j'): # joint speed - _, self.left_wheel._speed, self.right_wheel._speed = self.packeter.unpacketC2F() + _, self.left_wheel._speed, self.right_wheel._speed = self._packeter.unpacketC2F() elif code == ord('l'): # line sensor - _, self.left_line, self.center_line, self.right_line = self.packeter.unpacketC3I() + _, self._left_line, self._center_line, self._right_line = self._packeter.unpacketC3I() elif code == ord('c'): # color sensor - _, self.red, self.green, self.blue = self.packeter.unpacketC3I() + _, self._red, self._green, self._blue = self._packeter.unpacketC3I() elif code == ord('i'): # imu - _, self.ax, self.ay, self.az, self.gx, self.gy, self.gz = self.packeter.unpacketC6F() + _, self._ax, self._ay, self._az, self._gx, self._gy, self._gz = self._packeter.unpacketC6F() elif code == ord('p'): # battery percentage - _, self.battery_perc = self.packeter.unpacketC1F() + _, self._battery_perc = self._packeter.unpacketC1F() elif code == ord('d'): # distance sensor - _, self.left_tof, self.center_tof, self.right_tof = self.packeter.unpacketC3I() + _, self._left_tof, self._center_tof, self._right_tof = self._packeter.unpacketC3I() elif code == ord('t'): # touch input - _, self.touch_bits = self.packeter.unpacketC1B() + _, self._touch_byte = self._packeter.unpacketC1B() elif code == ord('b'): # behaviour - _, self.behaviour = self.packeter.unpacketC1B() + _, self._behaviour = self._packeter.unpacketC1B() elif code == ord('f'): # tof matrix - (_, self.left_tof, self.center_left_tof, self.center_tof, - self.center_right_tof, self.right_tof, self.top_tof, self.bottom_tof) = self.packeter.unpacketC7I() + (_, self._left_tof, self._center_left_tof, self._center_tof, + self._center_right_tof, self._right_tof, self._top_tof, self._bottom_tof) = self._packeter.unpacketC7I() elif code == ord('q'): # imu position - _, self.roll, self.pitch, self.yaw = self.packeter.unpacketC3F() + _, self._roll, self._pitch, self._yaw = self._packeter.unpacketC3F() elif code == ord('w'): # wheels position - _, self.left_wheel._position, self.right_wheel._position = self.packeter.unpacketC2F() + _, self.left_wheel._position, self.right_wheel._position = self._packeter.unpacketC2F() elif code == ord('v'): # robot velocity - _, self.linear_velocity, self.angular_velocity = self.packeter.unpacketC2F() + _, self._linear_velocity, self._angular_velocity = self._packeter.unpacketC2F() elif code == ord('x'): # robot ack - _, self.last_ack = self.packeter.unpacketC1B() + _, self._angular_velocity = self._packeter.unpacketC1B() elif code == ord('z'): # robot ack - _, self.x, self.y, self.theta = self.packeter.unpacketC3F() + _, self._x, self._y, self._theta = self._packeter.unpacketC3F() elif code == 0x7E: # firmware version - _, *self.version = self.packeter.unpacketC3B() + _, *self._version = self._packeter.unpacketC3B() else: return -1 @@ -614,72 +614,73 @@ def get_battery_charge(self) -> int: Returns the battery SOC :return: """ - if self.battery_perc > 100: + if self._battery_perc > 100: return 100 - return round(self.battery_perc) + return round(self._battery_perc) - def _get_touch(self) -> int: + @property + def _touch_bits(self) -> int: """ Returns the touch sensor's state :return: touch_bits """ - return self.touch_bits + return (self._touch_byte & 0xFF) if self._touch_byte is not None else 0x00 def get_touch_any(self) -> bool: """ Returns true if any button is pressed :return: """ - return bool(self.touch_bits & 0b00000001) + return bool(self._touch_bits & 0b00000001) def get_touch_ok(self) -> bool: """ Returns true if ok button is pressed :return: """ - return bool(self.touch_bits & 0b00000010) + return bool(self._touch_bits & 0b00000010) def get_touch_cancel(self) -> bool: """ Returns true if cancel button is pressed :return: """ - return bool(self.touch_bits & 0b00000100) + return bool(self._touch_bits & 0b00000100) def get_touch_center(self) -> bool: """ Returns true if center button is pressed :return: """ - return bool(self.touch_bits & 0b00001000) + return bool(self._touch_bits & 0b00001000) def get_touch_up(self) -> bool: """ Returns true if up button is pressed :return: """ - return bool(self.touch_bits & 0b00010000) + return bool(self._touch_bits & 0b00010000) def get_touch_left(self) -> bool: """ Returns true if left button is pressed :return: """ - return bool(self.touch_bits & 0b00100000) + return bool(self._touch_bits & 0b00100000) def get_touch_down(self) -> bool: """ Returns true if down button is pressed :return: """ - return bool(self.touch_bits & 0b01000000) + return bool(self._touch_bits & 0b01000000) def get_touch_right(self) -> bool: """ Returns true if right button is pressed :return: """ - return bool(self.touch_bits & 0b10000000) + return bool(self._touch_bits & 0b10000000) @staticmethod def _limit(value: float, lower: float, upper: float) -> float: @@ -768,7 +769,7 @@ def get_color_raw(self) -> (int, int, int): :return: red, green, blue """ - return self.red, self.green, self.blue + return self._red, self._green, self._blue def _normalize_color(self, r: float, g: float, b: float) -> (float, float, float): """ @@ -836,6 +837,9 @@ def get_color(self, color_format: str = 'rgb') -> (float, float, float): """ assert color_format in ['rgb', 'hsv'] + if None in list(self.get_color_raw()): + return None, None, None + if color_format == 'rgb': return self._normalize_color(*self.get_color_raw()) elif color_format == 'hsv': @@ -858,6 +862,9 @@ def hsv2label(h, s, v) -> str: :return: """ + if None in [h, s, v]: + return 'UNDEFINED' + if s < 0.1: if v < 0.05: label = 'BLACK' @@ -899,11 +906,15 @@ def get_distance(self, unit: str = 'cm') -> (float, float, float, float, float): :param unit: distance output unit :return: left_tof, center_left_tof, center_tof, center_right_tof, right_tof """ - return (convert_distance(self.left_tof, 'mm', unit), - convert_distance(self.center_left_tof, 'mm', unit), - convert_distance(self.center_tof, 'mm', unit), - convert_distance(self.center_right_tof, 'mm', unit), - convert_distance(self.right_tof, 'mm', unit)) + + if None in [self._left_tof, self._center_left_tof, self._center_tof, self._center_right_tof, self._right_tof]: + return None, None, None, None, None + + return (convert_distance(self._left_tof, 'mm', unit), + convert_distance(self._center_left_tof, 'mm', unit), + convert_distance(self._center_tof, 'mm', unit), + convert_distance(self._center_right_tof, 'mm', unit), + convert_distance(self._right_tof, 'mm', unit)) def get_distance_top(self, unit: str = 'cm') -> float: """ @@ -911,7 +922,7 @@ def get_distance_top(self, unit: str = 'cm') -> float: :param unit: :return: """ - return convert_distance(self.top_tof, 'mm', unit) + return convert_distance(self._top_tof, 'mm', unit) def get_distance_bottom(self, unit: str = 'cm') -> float: """ @@ -919,14 +930,14 @@ def get_distance_bottom(self, unit: str = 'cm') -> float: :param unit: :return: """ - return convert_distance(self.bottom_tof, 'mm', unit) + return convert_distance(self._bottom_tof, 'mm', unit) def get_version(self) -> str: """ Returns the firmware version of the Alvik :return: """ - return f'{self.version[0]}.{self.version[1]}.{self.version[2]}' + return f'{self._version[0]}.{self._version[1]}.{self._version[2]}' def print_status(self): """ From 9dc9394321dd11b8047dff5000671605673c289b Mon Sep 17 00:00:00 2001 From: Lucio Rossi Date: Mon, 26 Feb 2024 12:43:56 +0100 Subject: [PATCH 21/25] fix: lost _last_ack --- arduino_alvik.py | 10 +++++----- 1 file changed, 5 insertions(+), 5 deletions(-) diff --git a/arduino_alvik.py b/arduino_alvik.py index 4705a96..07329a8 100644 --- a/arduino_alvik.py +++ b/arduino_alvik.py @@ -66,7 +66,7 @@ def __init__(self): self._bottom_tof = None self._linear_velocity = None self._angular_velocity = None - self._angular_velocity = '' + self._last_ack = '' self._version = [None, None, None] @staticmethod @@ -191,7 +191,7 @@ def _wait_for_ack(self) -> None: Waits until receives 0x00 ack from robot :return: """ - while self._angular_velocity != 0x00: + while self._last_ack != 0x00: sleep_ms(20) @staticmethod @@ -231,7 +231,7 @@ def is_target_reached(self) -> bool: It also responds with an ack received message :return: """ - if self._angular_velocity != ord('M') and self._angular_velocity != ord('R'): + if self._last_ack != ord('M') and self._last_ack != ord('R'): sleep_ms(50) return False else: @@ -476,7 +476,7 @@ def get_ack(self): Returns last acknowledgement :return: """ - return self._angular_velocity + return self._last_ack # def send_ack(self): # self._packeter.packetC1B(ord('X'), ACK_) @@ -597,7 +597,7 @@ def _parse_message(self) -> int: _, self._linear_velocity, self._angular_velocity = self._packeter.unpacketC2F() elif code == ord('x'): # robot ack - _, self._angular_velocity = self._packeter.unpacketC1B() + _, self._last_ack = self._packeter.unpacketC1B() elif code == ord('z'): # robot ack _, self._x, self._y, self._theta = self._packeter.unpacketC3F() From b48fc96c188b1f1d79c9ce81d85bfeab3e4477eb Mon Sep 17 00:00:00 2001 From: Lucio Rossi Date: Mon, 26 Feb 2024 16:32:40 +0100 Subject: [PATCH 22/25] mod: _idle updating every second --- arduino_alvik.py | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/arduino_alvik.py b/arduino_alvik.py index 01c6fa6..9ca8178 100644 --- a/arduino_alvik.py +++ b/arduino_alvik.py @@ -173,7 +173,7 @@ def begin(self) -> int: if not self.is_on(): print("\nTurn on your Arduino Alvik!\n") sleep_ms(1000) - self._idle() + self._idle(1000) self._begin_update_thread() sleep_ms(100) self._reset_hw() @@ -526,7 +526,7 @@ def _update(self, delay_=1): while True: if not self.is_on(): print("Alvik is off") - self._idle(delay_, check_on_thread=True) + self._idle(1000, check_on_thread=True) self._reset_hw() self._flush_uart() sleep_ms(1000) From 921e59be147e07872e2d5e623f967c0f9bbd5af9 Mon Sep 17 00:00:00 2001 From: Lucio Rossi Date: Mon, 26 Feb 2024 16:35:14 +0100 Subject: [PATCH 23/25] mod: ALVIKDEV-68 lower charge threshold to 97 --- arduino_alvik.py | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/arduino_alvik.py b/arduino_alvik.py index 9ca8178..b0b9b38 100644 --- a/arduino_alvik.py +++ b/arduino_alvik.py @@ -85,7 +85,7 @@ def _progress_bar(percentage: float) -> None: :return: """ sys.stdout.write('\r') - if percentage > 98: + if percentage > 97: marks_str = ' \U0001F50B' else: marks_str = ' \U0001FAAB' @@ -121,7 +121,7 @@ def _idle(self, delay_=1, check_on_thread=False) -> None: soc_perc = soc_raw*0.00390625 self._progress_bar(round(soc_perc)) sleep_ms(delay_) - if soc_perc > 98: + if soc_perc > 97: LEDG.value(0) LEDR.value(1) else: From 6538c588a3c37fbd35d586f2aa9efbaff98661fd Mon Sep 17 00:00:00 2001 From: Lucio Rossi Date: Mon, 26 Feb 2024 18:01:02 +0100 Subject: [PATCH 24/25] exmaples: pose_example.py with wait if non-blocking --- examples/pose_example.py | 24 ++++++++++++++++++++++++ 1 file changed, 24 insertions(+) diff --git a/examples/pose_example.py b/examples/pose_example.py index 3004d4d..ab248e9 100644 --- a/examples/pose_example.py +++ b/examples/pose_example.py @@ -34,15 +34,39 @@ alvik.move(50.0, 'mm', blocking=False) print("on target after move") + while not alvik.is_target_reached(): + alvik.left_led.set_color(1, 0, 0) + sleep_ms(500) + alvik.left_led.set_color(0, 0, 0) + sleep_ms(500) + alvik.rotate(45.0, 'deg', blocking=False) print("on target after rotation") + while not alvik.is_target_reached(): + alvik.left_led.set_color(1, 0, 0) + sleep_ms(500) + alvik.left_led.set_color(0, 0, 0) + sleep_ms(500) + alvik.move(100.0, 'mm', blocking=False) print("on target after move") + while not alvik.is_target_reached(): + alvik.left_led.set_color(1, 0, 0) + sleep_ms(500) + alvik.left_led.set_color(0, 0, 0) + sleep_ms(500) + alvik.rotate(-90.00, 'deg', blocking=False) print("on target after rotation") + while not alvik.is_target_reached(): + alvik.left_led.set_color(1, 0, 0) + sleep_ms(500) + alvik.left_led.set_color(0, 0, 0) + sleep_ms(500) + x, y, theta = alvik.get_pose() print(f'Current pose is x(cm)={x}, y(cm)={y}, theta(deg)={theta}') From a67e376f9b1c7d81fafad0e8b4940b7e3f58176b Mon Sep 17 00:00:00 2001 From: Lucio Rossi Date: Tue, 27 Feb 2024 12:46:15 +0100 Subject: [PATCH 25/25] improvements: getters have guards on None values for alvik params typing reformat better types and type-hints --- arduino_alvik.py | 108 ++++++++++++++++++++++++----------------------- conversions.py | 4 ++ 2 files changed, 60 insertions(+), 52 deletions(-) diff --git a/arduino_alvik.py b/arduino_alvik.py index d7b7ca5..8ae435c 100644 --- a/arduino_alvik.py +++ b/arduino_alvik.py @@ -29,7 +29,7 @@ def __init__(self): self._packeter = ucPack(200) self.left_wheel = _ArduinoAlvikWheel(self._packeter, ord('L')) self.right_wheel = _ArduinoAlvikWheel(self._packeter, ord('R')) - self._led_state = [None] + self._led_state = list((None,)) self.left_led = _ArduinoAlvikRgbLed(self._packeter, 'left', self._led_state, rgb_mask=[0b00000100, 0b00001000, 0b00010000]) self.right_led = _ArduinoAlvikRgbLed(self._packeter, 'right', self._led_state, @@ -84,12 +84,13 @@ def _progress_bar(percentage: float) -> None: :param percentage: :return: """ - sys.stdout.write('\r') + sys.stdout.write(bytes('\r'.encode('utf-8'))) if percentage > 97: marks_str = ' \U0001F50B' else: marks_str = ' \U0001FAAB' - sys.stdout.write(marks_str + f" {percentage}% \t") + word = marks_str + f" {percentage}% \t" + sys.stdout.write(bytes((word.encode('utf-8')))) def _idle(self, delay_=1, check_on_thread=False) -> None: """ @@ -105,20 +106,20 @@ def _idle(self, delay_=1, check_on_thread=False) -> None: while not self.is_on(): if check_on_thread and not self.__class__._update_thread_running: break - ESP32_SDA = Pin(A4, Pin.OUT) - ESP32_SCL = Pin(A5, Pin.OUT) - ESP32_SCL.value(1) - ESP32_SDA.value(1) + _ESP32_SDA = Pin(A4, Pin.OUT) + _ESP32_SCL = Pin(A5, Pin.OUT) + _ESP32_SCL.value(1) + _ESP32_SDA.value(1) sleep_ms(100) - ESP32_SCL.value(0) - ESP32_SDA.value(0) + _ESP32_SCL.value(0) + _ESP32_SDA.value(0) cmd = bytearray(1) cmd[0] = 0x06 i2c = I2C(0, scl=ESP32_SCL, sda=ESP32_SDA) i2c.writeto(0x36, cmd) soc_raw = struct.unpack('h', i2c.readfrom(0x36, 2))[0] - soc_perc = soc_raw*0.00390625 + soc_perc = soc_raw * 0.00390625 self._progress_bar(round(soc_perc)) sleep_ms(delay_) if soc_perc > 97: @@ -140,30 +141,32 @@ def _idle(self, delay_=1, check_on_thread=False) -> None: LEDG.value(1) NANO_CHK.value(0) - def _snake_robot(self, duration: int = 1000): + @staticmethod + def _snake_robot(duration: int = 1000): """ Snake robot animation - :param percentage: + :param duration: :return: """ - i = 0 robot = '\U0001F916' snake = '\U0001F40D' cycles = int(duration / 200) + frame = '' for i in range(0, cycles): - sys.stdout.write('\r') + sys.stdout.write(bytes('\r'.encode('utf-8'))) pre = ' ' * i between = ' ' * (i % 2 + 1) post = ' ' * 5 frame = pre + snake + between + robot + post - sys.stdout.write(frame) + sys.stdout.write(bytes(frame.encode('utf-8'))) sleep_ms(200) - sys.stdout.write('\r') - sys.stdout.write('') + sys.stdout.write(bytes('\r'.encode('utf-8'))) + clear_frame = ' ' * len(frame) + sys.stdout.write(bytes(clear_frame.encode('utf-8'))) def begin(self) -> int: """ @@ -309,7 +312,7 @@ def _reset_hw(): RESET_STM32.value(1) sleep_ms(100) - def get_wheels_speed(self, unit: str = 'rpm') -> (float, float): + def get_wheels_speed(self, unit: str = 'rpm') -> (float | None, float | None): """ Returns the speed of the wheels :param unit: the speed unit of measurement (default: 'rpm') @@ -327,8 +330,8 @@ def set_wheels_speed(self, left_speed: float, right_speed: float, unit: str = 'r """ if unit == '%': - left_speed = (left_speed/100)*MOTOR_MAX_RPM - right_speed = (right_speed/100)*MOTOR_MAX_RPM + left_speed = (left_speed / 100) * MOTOR_MAX_RPM + right_speed = (right_speed / 100) * MOTOR_MAX_RPM else: left_speed = convert_rotational_speed(left_speed, unit, 'rpm') right_speed = convert_rotational_speed(right_speed, unit, 'rpm') @@ -345,10 +348,10 @@ def set_wheels_position(self, left_angle: float, right_angle: float, unit: str = :return: """ self._packeter.packetC2F(ord('A'), convert_angle(left_angle, unit, 'deg'), - convert_angle(right_angle, unit, 'deg')) + convert_angle(right_angle, unit, 'deg')) uart.write(self._packeter.msg[0:self._packeter.msg_size]) - def get_wheels_position(self, unit: str = 'deg') -> (float, float): + def get_wheels_position(self, unit: str = 'deg') -> (float | None, float | None): """ Returns the angle of the wheels :param unit: the angle unit of measurement (default: 'deg') @@ -357,7 +360,7 @@ def get_wheels_position(self, unit: str = 'deg') -> (float, float): return (convert_angle(self.left_wheel.get_position(), 'deg', unit), convert_angle(self.right_wheel.get_position(), 'deg', unit)) - def get_orientation(self) -> (float, float, float): + def get_orientation(self) -> (float | None, float | None, float | None): """ Returns the orientation of the IMU :return: roll, pitch, yaw @@ -365,28 +368,28 @@ def get_orientation(self) -> (float, float, float): return self._roll, self._pitch, self._yaw - def get_accelerations(self) -> (float, float, float): + def get_accelerations(self) -> (float | None, float | None, float | None): """ Returns the 3-axial acceleration of the IMU :return: ax, ay, az """ return self._ax, self._ay, self._az - def get_gyros(self) -> (float, float, float): + def get_gyros(self) -> (float | None, float | None, float | None): """ Returns the 3-axial angular acceleration of the IMU :return: gx, gy, gz """ return self._gx, self._gy, self._gz - def get_imu(self) -> (float, float, float, float, float, float): + def get_imu(self) -> (float | None, float | None, float | None, float | None, float | None, float | None): """ Returns all the IMUs readouts :return: ax, ay, az, gx, gy, gz """ return self._ax, self._ay, self._az, self._gx, self._gy, self._gz - def get_line_sensors(self) -> (int, int, int): + def get_line_sensors(self) -> (int | None, int | None, int | None): """ Returns the line sensors readout :return: left_line, center_line, right_line @@ -406,7 +409,7 @@ def drive(self, linear_velocity: float, angular_velocity: float, linear_unit: st """ linear_velocity = convert_speed(linear_velocity, linear_unit, 'mm/s') if angular_unit == '%': - angular_velocity = (angular_velocity/100)*ROBOT_MAX_DEG_S + angular_velocity = (angular_velocity / 100) * ROBOT_MAX_DEG_S else: angular_velocity = convert_rotational_speed(angular_velocity, angular_unit, 'deg/s') self._packeter.packetC2F(ord('V'), linear_velocity, angular_velocity) @@ -419,7 +422,7 @@ def brake(self): """ self.drive(0, 0) - def get_drive_speed(self, linear_unit: str = 'cm/s', angular_unit: str = 'deg/s') -> (float, float): + def get_drive_speed(self, linear_unit: str = 'cm/s', angular_unit: str = 'deg/s') -> (float | None, float | None): """ Returns linear and angular velocity of the robot :param linear_unit: output linear velocity unit of meas @@ -427,7 +430,8 @@ def get_drive_speed(self, linear_unit: str = 'cm/s', angular_unit: str = 'deg/s' :return: linear_velocity, angular_velocity """ if angular_unit == '%': - angular_velocity = (self._angular_velocity/ROBOT_MAX_DEG_S)*100 + angular_velocity = (self._angular_velocity / ROBOT_MAX_DEG_S) * 100 \ + if self._angular_velocity is not None else None else: angular_velocity = convert_rotational_speed(self._angular_velocity, 'deg/s', angular_unit) @@ -450,7 +454,8 @@ def reset_pose(self, x: float, y: float, theta: float, distance_unit: str = 'cm' uart.write(self._packeter.msg[0:self._packeter.msg_size]) sleep_ms(1000) - def get_pose(self, distance_unit: str = 'cm', angle_unit: str = 'deg') -> (float, float, float): + def get_pose(self, distance_unit: str = 'cm', angle_unit: str = 'deg') \ + -> (float | None, float | None, float | None): """ Returns the current pose of the robot :param distance_unit: unit of x and y outputs @@ -471,7 +476,7 @@ def set_servo_positions(self, a_position: int, b_position: int): self._packeter.packetC2B(ord('S'), a_position & 0xFF, b_position & 0xFF) uart.write(self._packeter.msg[0:self._packeter.msg_size]) - def get_ack(self): + def get_ack(self) -> str: """ Returns last acknowledgement :return: @@ -609,11 +614,13 @@ def _parse_message(self) -> int: return 0 - def get_battery_charge(self) -> int: + def get_battery_charge(self) -> int | None: """ Returns the battery SOC :return: """ + if self._battery_perc is None: + return None if self._battery_perc > 100: return 100 return round(self._battery_perc) @@ -729,9 +736,9 @@ def color_calibration(self, background: str = 'white') -> None: blue_avg += blue sleep_ms(10) - red_avg = int(red_avg/100) - green_avg = int(green_avg/100) - blue_avg = int(blue_avg/100) + red_avg = int(red_avg / 100) + green_avg = int(green_avg / 100) + blue_avg = int(blue_avg / 100) if background == 'white': self._white_cal = [red_avg, green_avg, blue_avg] @@ -763,7 +770,7 @@ def color_calibration(self, background: str = 'white') -> None: for line in lines: file.write(line) - def get_color_raw(self) -> (int, int, int): + def get_color_raw(self) -> (int | None, int | None, int | None): """ Returns the color sensor's raw readout :return: red, green, blue @@ -783,9 +790,9 @@ def _normalize_color(self, r: float, g: float, b: float) -> (float, float, float g = self._limit(g, self._black_cal[1], self._white_cal[1]) b = self._limit(b, self._black_cal[2], self._white_cal[2]) - r = (r - self._black_cal[0])/(self._white_cal[0] - self._black_cal[0]) - g = (g - self._black_cal[1])/(self._white_cal[1] - self._black_cal[1]) - b = (b - self._black_cal[2])/(self._white_cal[2] - self._black_cal[2]) + r = (r - self._black_cal[0]) / (self._white_cal[0] - self._black_cal[0]) + g = (g - self._black_cal[1]) / (self._white_cal[1] - self._black_cal[1]) + b = (b - self._black_cal[2]) / (self._white_cal[2] - self._black_cal[2]) return r, g, b @@ -829,7 +836,7 @@ def rgb2hsv(r: float, g: float, b: float) -> (float, float, float): return h, s, v - def get_color(self, color_format: str = 'rgb') -> (float, float, float): + def get_color(self, color_format: str = 'rgb') -> (float | None, float | None, float | None): """ Returns the normalized color readout of the color sensor :param color_format: rgb or hsv only @@ -888,7 +895,7 @@ def hsv2label(h, s, v) -> str: label = 'BLUE' elif 260 <= h < 280: label = 'VIOLET' - else: # h<20 or h>=280 is more problematic + else: # h<20 or h>=280 is more problematic if v < 0.5 and s < 0.45: label = 'BROWN' else: @@ -900,23 +907,20 @@ def hsv2label(h, s, v) -> str: label = 'BLACK' return label - def get_distance(self, unit: str = 'cm') -> (float, float, float, float, float): + def get_distance(self, unit: str = 'cm') -> (float | None, float | None, float | None, float | None, float | None): """ Returns the distance readout of the TOF sensor :param unit: distance output unit :return: left_tof, center_left_tof, center_tof, center_right_tof, right_tof """ - if None in [self._left_tof, self._center_left_tof, self._center_tof, self._center_right_tof, self._right_tof]: - return None, None, None, None, None - return (convert_distance(self._left_tof, 'mm', unit), convert_distance(self._center_left_tof, 'mm', unit), convert_distance(self._center_tof, 'mm', unit), convert_distance(self._center_right_tof, 'mm', unit), convert_distance(self._right_tof, 'mm', unit)) - def get_distance_top(self, unit: str = 'cm') -> float: + def get_distance_top(self, unit: str = 'cm') -> float | None: """ Returns the obstacle top distance readout :param unit: @@ -924,7 +928,7 @@ def get_distance_top(self, unit: str = 'cm') -> float: """ return convert_distance(self._top_tof, 'mm', unit) - def get_distance_bottom(self, unit: str = 'cm') -> float: + def get_distance_bottom(self, unit: str = 'cm') -> float | None: """ Returns the obstacle bottom distance readout :param unit: @@ -998,26 +1002,26 @@ def set_speed(self, velocity: float, unit: str = 'rpm'): """ if unit == '%': - velocity = (velocity/100)*MOTOR_MAX_RPM + velocity = (velocity / 100) * MOTOR_MAX_RPM else: velocity = convert_rotational_speed(velocity, unit, 'rpm') self._packeter.packetC2B1F(ord('W'), self._label & 0xFF, ord('V'), velocity) uart.write(self._packeter.msg[0:self._packeter.msg_size]) - def get_speed(self, unit: str = 'rpm') -> float: + def get_speed(self, unit: str = 'rpm') -> float | None: """ Returns the current RPM speed of the wheel :param unit: the unit of the output speed :return: """ if unit == '%': - speed = (self._speed/MOTOR_MAX_RPM)*100 + speed = (self._speed / MOTOR_MAX_RPM) * 100 if self._speed is not None else None else: speed = convert_rotational_speed(self._speed, 'rpm', unit) return speed - def get_position(self, unit: str = 'deg') -> float: + def get_position(self, unit: str = 'deg') -> float | None: """ Returns the wheel position (angle with respect to the reference) :param unit: the unit of the output position diff --git a/conversions.py b/conversions.py index d2cc96c..36486ce 100644 --- a/conversions.py +++ b/conversions.py @@ -9,6 +9,10 @@ def wrapper(*args, **kwargs): return func(*args, **kwargs) except KeyError: raise ConversionError(f'Cannot {func.__name__} from {args[1]} to {args[2]}') + except TypeError: + return None + except Exception as e: + raise ConversionError(f'Unexpected error: {e}') return wrapper