From 0e2c7745fde3c27bfa337ceb1f1b137f6cfefa17 Mon Sep 17 00:00:00 2001 From: Lucio Rossi Date: Mon, 22 Jan 2024 09:48:40 +0100 Subject: [PATCH 01/31] feat: alvik.begin method starts main thread and resets STM32 --- arduino_alvik.py | 9 +++++++-- examples/led_setting.py | 5 +---- examples/message_reader.py | 5 +---- examples/move_example.py | 6 +----- examples/read_color_sensor.py | 5 +---- examples/read_touch.py | 5 +---- examples/set_pid.py | 5 +---- 7 files changed, 13 insertions(+), 27 deletions(-) diff --git a/arduino_alvik.py b/arduino_alvik.py index f10dbe5..92d6c82 100644 --- a/arduino_alvik.py +++ b/arduino_alvik.py @@ -38,7 +38,12 @@ def __init__(self): self.bottom_tof = None self.version = [None, None, None] - def run(self): + def begin(self): + self._run() + sleep_ms(100) + self._reset_hw() + + def _run(self): """ Runs robot background operations (e.g. threaded update) :return: @@ -54,7 +59,7 @@ def stop(self): self._update_thread_running = False @staticmethod - def reset_hw(): + def _reset_hw(): """ Resets the STM32 :return: diff --git a/examples/led_setting.py b/examples/led_setting.py index cc2c005..ac73241 100644 --- a/examples/led_setting.py +++ b/examples/led_setting.py @@ -3,10 +3,7 @@ import sys alvik = ArduinoAlvik() - -alvik.run() -sleep_ms(100) -alvik.reset_hw() +alvik.begin() while True: try: diff --git a/examples/message_reader.py b/examples/message_reader.py index 89086f8..b8216ff 100644 --- a/examples/message_reader.py +++ b/examples/message_reader.py @@ -3,10 +3,7 @@ import sys alvik = ArduinoAlvik() - -alvik.run() -sleep_ms(100) -alvik.reset_hw() +alvik.begin() speed = 0 while True: diff --git a/examples/move_example.py b/examples/move_example.py index ffc674b..fcb0a62 100644 --- a/examples/move_example.py +++ b/examples/move_example.py @@ -3,11 +3,7 @@ import sys alvik = ArduinoAlvik() - -alvik.run() -sleep_ms(100) -alvik.reset_hw() - +alvik.begin() while True: try: diff --git a/examples/read_color_sensor.py b/examples/read_color_sensor.py index aa4d1a4..5bf597c 100644 --- a/examples/read_color_sensor.py +++ b/examples/read_color_sensor.py @@ -3,10 +3,7 @@ import sys alvik = ArduinoAlvik() - -alvik.run() -sleep_ms(1000) -alvik.reset_hw() +alvik.begin() speed = 0 while True: diff --git a/examples/read_touch.py b/examples/read_touch.py index 8ebf4a2..abc0d5c 100644 --- a/examples/read_touch.py +++ b/examples/read_touch.py @@ -3,10 +3,7 @@ import sys alvik = ArduinoAlvik() - -alvik.run() -sleep_ms(1000) -alvik.reset_hw() +alvik.begin() speed = 0 while True: diff --git a/examples/set_pid.py b/examples/set_pid.py index a3d21b1..ad480f3 100644 --- a/examples/set_pid.py +++ b/examples/set_pid.py @@ -3,10 +3,7 @@ import sys alvik = ArduinoAlvik() - -alvik.run() -sleep_ms(1000) -alvik.reset_hw() +alvik.begin() speed = 0 while True: From f6b304603065c54e16ea829a8f0769fe9e2389b2 Mon Sep 17 00:00:00 2001 From: Lucio Rossi Date: Mon, 22 Jan 2024 11:06:13 +0100 Subject: [PATCH 02/31] mod: begin checks if alvik is on --- arduino_alvik.py | 5 ++++- examples/message_reader.py | 4 +++- pinout_definitions.py | 10 ++++++---- 3 files changed, 13 insertions(+), 6 deletions(-) diff --git a/arduino_alvik.py b/arduino_alvik.py index 92d6c82..96c762d 100644 --- a/arduino_alvik.py +++ b/arduino_alvik.py @@ -38,7 +38,10 @@ def __init__(self): self.bottom_tof = None self.version = [None, None, None] - def begin(self): + def begin(self) -> int: + if not CHECK_STM32.value(): + print("\nTurn on your Arduino Alvik!\n") + return -1 self._run() sleep_ms(100) self._reset_hw() diff --git a/examples/message_reader.py b/examples/message_reader.py index b8216ff..219d226 100644 --- a/examples/message_reader.py +++ b/examples/message_reader.py @@ -3,7 +3,9 @@ import sys alvik = ArduinoAlvik() -alvik.begin() +if alvik.begin() < 0: + sys.exit() + speed = 0 while True: diff --git a/pinout_definitions.py b/pinout_definitions.py index b10a06a..e9fe791 100644 --- a/pinout_definitions.py +++ b/pinout_definitions.py @@ -1,7 +1,9 @@ from machine import Pin # NANO to STM32 PINS -D2 = 5 # ESP32 pin5 -> nano D2 -D3 = 6 # ESP32 pin6 -> nano D3 -BOOT0_STM32 = Pin(D2, Pin.OUT) # nano D2 -> STM32 Boot0 -RESET_STM32 = Pin(D3, Pin.OUT) # nano D2 -> STM32 NRST +D2 = 5 # ESP32 pin5 -> nano D2 +D3 = 6 # ESP32 pin6 -> nano D3 +A6 = 13 # ESP32 pin13 -> nano A6/D23 +BOOT0_STM32 = Pin(D2, Pin.OUT) # nano D2 -> STM32 Boot0 +RESET_STM32 = Pin(D3, Pin.OUT) # nano D2 -> STM32 NRST +CHECK_STM32 = Pin(A6, Pin.IN, Pin.PULL_UP) # nano A6/D23 -> STM32 ROBOT_CHK From e29e429f17e4c27dfae7e7607b3a07af7f4905b0 Mon Sep 17 00:00:00 2001 From: Lucio Rossi Date: Mon, 22 Jan 2024 11:49:50 +0100 Subject: [PATCH 03/31] mod: upd_thread methods naming --- arduino_alvik.py | 16 +++++++++++++--- 1 file changed, 13 insertions(+), 3 deletions(-) diff --git a/arduino_alvik.py b/arduino_alvik.py index 96c762d..f2bf457 100644 --- a/arduino_alvik.py +++ b/arduino_alvik.py @@ -42,11 +42,12 @@ def begin(self) -> int: if not CHECK_STM32.value(): print("\nTurn on your Arduino Alvik!\n") return -1 - self._run() + self._begin_update_thread() sleep_ms(100) self._reset_hw() + return 0 - def _run(self): + def _begin_update_thread(self): """ Runs robot background operations (e.g. threaded update) :return: @@ -54,13 +55,22 @@ def _run(self): self._update_thread_running = True self._update_thread_id = _thread.start_new_thread(self._update, (1,)) - def stop(self): + def _stop_update_thread(self): """ Stops the background operations :return: """ self._update_thread_running = False + def stop(self): + """ + Stops all Alvik operations + :return: + """ + # stop engines + # turn off UI leds + self._stop_update_thread() + @staticmethod def _reset_hw(): """ From de14198962f7170c4a5c0eef42eda3643ef45166 Mon Sep 17 00:00:00 2001 From: Lucio Rossi Date: Mon, 22 Jan 2024 17:33:20 +0100 Subject: [PATCH 04/31] feat: alvik wheels as children. mechanical constants in robot_definitions.py --- arduino_alvik.py | 64 ++++++++++++++++++++++++++++++++++++++++++++ robot_definitions.py | 7 +++++ 2 files changed, 71 insertions(+) create mode 100644 robot_definitions.py diff --git a/arduino_alvik.py b/arduino_alvik.py index f2bf457..fbff859 100644 --- a/arduino_alvik.py +++ b/arduino_alvik.py @@ -5,6 +5,7 @@ from ucPack import ucPack from pinout_definitions import * +from robot_definitions import * from constants import * @@ -12,6 +13,8 @@ class ArduinoAlvik: def __init__(self): self.packeter = ucPack(200) + self.left_wheel = ArduinoAlvikWheel(self.packeter, ord('L')) + self.right_wheel = ArduinoAlvikWheel(self.packeter, ord('R')) self._update_thread_running = False self._update_thread_id = None self.l_speed = None @@ -215,6 +218,8 @@ def _parse_message(self) -> int: if code == ord('j'): # joint speed _, self.l_speed, self.r_speed = self.packeter.unpacketC2F() + self.left_wheel._speed = self.l_speed + self.right_wheel._speed = self.r_speed elif code == ord('l'): # line sensor _, self.left_line, self.center_line, self.right_line = self.packeter.unpacketC3I() @@ -300,3 +305,62 @@ def print_status(self): if str(a).startswith('_'): continue print(f'{str(a).upper()} = {getattr(self, str(a))}') + + +class ArduinoAlvikWheel: + + def __init__(self, packeter: ucPack, label: int, wheel_diameter_mm: float = WHEEL_DIAMETER_MM): + self._packeter = packeter + self._label = label + self._wheel_diameter_mm = wheel_diameter_mm + self._speed = None + + def reset(self, initial_position: float = 0.0): + pass + + 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 + :param kp: proportional gain + :param ki: integration gain + :param kd: derivative gain + :return: + """ + + self._packeter.packetC1B3F(ord('P'), self._label, kp, ki, kd) + uart.write(self._packeter.msg[0:self._packeter.msg_size]) + + def stop(self): + """ + Stop Alvik wheel + :return: + """ + self.set_speed(0) + + def set_speed(self, velocity: float, unit: str = 'rpm'): + """ + Sets left/right motor speed + :param velocity: the speed of the motor + :param unit: the unit of measurement + :return: + """ + + 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): + """ + Gets the current RPM speed of the wheel + :return: + """ + return self._speed + + def set_position(self, position: float, unit: str = 'deg'): + """ + Sets left/right motor speed + :param position: the speed of the motor + :param unit: the unit of measurement + :return: + """ + self._packeter.packetC2B1F(ord('W'), self._label & 0xFF, ord('P'), position) + uart.write(self._packeter.msg[0:self._packeter.msg_size]) diff --git a/robot_definitions.py b/robot_definitions.py new file mode 100644 index 0000000..f48df90 --- /dev/null +++ b/robot_definitions.py @@ -0,0 +1,7 @@ +# Motor control and mechanical parameters +MOTOR_KP_DEFAULT = 32.0 +MOTOR_KI_DEFAULT = 450.0 +MOTOR_KD_DEFAULT = 0.0 + +# Wheels parameters +WHEEL_DIAMETER_MM = 34 From 117206ffe33aaae786ed147b539ecdd6c796d409 Mon Sep 17 00:00:00 2001 From: Lucio Rossi Date: Tue, 23 Jan 2024 12:57:48 +0100 Subject: [PATCH 05/31] feat: rgb leds as ArduinoAlvik children --- arduino_alvik.py | 90 ++++++++++++++++++++++++++++-------- examples/rgb_leds_setting.py | 40 ++++++++++++++++ 2 files changed, 111 insertions(+), 19 deletions(-) create mode 100644 examples/rgb_leds_setting.py diff --git a/arduino_alvik.py b/arduino_alvik.py index fbff859..8dfba3f 100644 --- a/arduino_alvik.py +++ b/arduino_alvik.py @@ -15,6 +15,9 @@ 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, rgb_mask=[0b00000100, 0b00001000, 0b00010000]) + self.right_led = ArduinoAlvikRgbLed(self.packeter, 'right', self.led_state, rgb_mask=[0b00100000, 0b01000000, 0b10000000]) self._update_thread_running = False self._update_thread_id = None self.l_speed = None @@ -22,7 +25,6 @@ def __init__(self): self.battery_perc = None self.touch_bits = None self.behaviour = None - self.led_state = None self.red = None self.green = None self.blue = None @@ -149,37 +151,61 @@ def _set_leds(self, led_state: int): 5->right_red 6->right_green 7->right_blue :return: """ - self.led_state = led_state & 0xFF - self.packeter.packetC1B(ord('L'), self.led_state) + 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): - if self.led_state is None: + """ + Turns on/off the builtin led + :param value: + :return: + """ + if self.led_state[0] is None: self._set_leds(0x00) - self.led_state = self.led_state | 0b00000001 if value else self.led_state & 0b11111110 - self._set_leds(self.led_state) + 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): - if self.led_state is None: + """ + Turns on/off the illuminator led + :param value: + :return: + """ + if self.led_state[0] is None: self._set_leds(0x00) - self.led_state = self.led_state | 0b00000010 if value else self.led_state & 0b11111101 - self._set_leds(self.led_state) + 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 set_left_led_color(self, red: bool, green: bool, blue: bool): - if self.led_state is None: + """ + Sets the r,g,b state of the left LED + :param red: + :param green: + :param blue: + :return: + """ + if self.led_state[0] is None: self._set_leds(0x00) - self.led_state = self.led_state | 0b00000100 if red else self.led_state & 0b11111011 - self.led_state = self.led_state | 0b00001000 if green else self.led_state & 0b11110111 - self.led_state = self.led_state | 0b00010000 if blue else self.led_state & 0b11101111 - self._set_leds(self.led_state) + self.led_state[0] = self.led_state[0] | 0b00000100 if red else self.led_state[0] & 0b11111011 + self.led_state[0] = self.led_state[0] | 0b00001000 if green else self.led_state[0] & 0b11110111 + self.led_state[0] = self.led_state[0] | 0b00010000 if blue else self.led_state[0] & 0b11101111 + self._set_leds(self.led_state[0]) def set_right_led_color(self, red: bool, green: bool, blue: bool): - if self.led_state is None: + """ + Sets the r,g,b state of the right LED + :param red: + :param green: + :param blue: + :return: + """ + if self.led_state[0] is None: self._set_leds(0x00) - self.led_state = self.led_state | 0b00100000 if red else self.led_state & 0b11011111 - self.led_state = self.led_state | 0b01000000 if green else self.led_state & 0b10111111 - self.led_state = self.led_state | 0b10000000 if blue else self.led_state & 0b01111111 - self._set_leds(self.led_state) + self.led_state[0] = self.led_state[0] | 0b00100000 if red else self.led_state[0] & 0b11011111 + self.led_state[0] = self.led_state[0] | 0b01000000 if green else self.led_state[0] & 0b10111111 + self.led_state[0] = self.led_state[0] | 0b10000000 if blue else self.led_state[0] & 0b01111111 + self._set_leds(self.led_state[0]) def _update(self, delay_=1): """ @@ -364,3 +390,29 @@ def set_position(self, position: float, unit: str = 'deg'): """ self._packeter.packetC2B1F(ord('W'), self._label & 0xFF, ord('P'), position) uart.write(self._packeter.msg[0:self._packeter.msg_size]) + + +class ArduinoAlvikRgbLed: + def __init__(self, packeter: ucPack, label: str, led_state: list[int], rgb_mask: list[int]): + self._packeter = packeter + self.label = label + self._rgb_mask = rgb_mask + self._led_state = led_state + + def set_color(self, red: bool, green: bool, blue: bool): + """ + Sets the LED's r,g,b state + :param red: + :param green: + :param blue: + :return: + """ + led_status = self._led_state[0] + if led_status is None: + return + led_status = led_status | self._rgb_mask[0] if red else led_status & (0b11111111 - self._rgb_mask[0]) + led_status = led_status | self._rgb_mask[1] if green else led_status & (0b11111111 - self._rgb_mask[1]) + led_status = led_status | self._rgb_mask[2] if blue else led_status & (0b11111111 - self._rgb_mask[2]) + self._led_state[0] = led_status + self._packeter.packetC1B(ord('L'), led_status & 0xFF) + uart.write(self._packeter.msg[0:self._packeter.msg_size]) diff --git a/examples/rgb_leds_setting.py b/examples/rgb_leds_setting.py new file mode 100644 index 0000000..07c5781 --- /dev/null +++ b/examples/rgb_leds_setting.py @@ -0,0 +1,40 @@ +from arduino_alvik import ArduinoAlvik +from time import sleep_ms +import sys + +alvik = ArduinoAlvik() +alvik.begin() + +while True: + try: + alvik._set_leds(0xff) + sleep_ms(1000) + alvik._set_leds(0x00) + sleep_ms(1000) + alvik.set_builtin_led(1) + sleep_ms(1000) + alvik.set_illuminator(1) + sleep_ms(1000) + alvik.set_builtin_led(0) + sleep_ms(1000) + alvik.set_illuminator(0) + sleep_ms(1000) + alvik.left_led.set_color(0, 0, 1) + sleep_ms(1000) + alvik.left_led.set_color(0, 1, 0) + sleep_ms(1000) + alvik.left_led.set_color(1, 0, 0) + sleep_ms(1000) + alvik.left_led.set_color(1, 1, 1) + sleep_ms(1000) + alvik.right_led.set_color(0, 0, 1) + sleep_ms(1000) + alvik.right_led.set_color(0, 1, 0) + sleep_ms(1000) + alvik.right_led.set_color(1, 0, 0) + sleep_ms(1000) + alvik.right_led.set_color(1, 1, 1) + sleep_ms(1000) + except KeyboardInterrupt as e: + print('over') + sys.exit() From 636687fe5529125f7ee60b174b16cafda72498e1 Mon Sep 17 00:00:00 2001 From: Lucio Rossi Date: Tue, 23 Jan 2024 17:31:02 +0100 Subject: [PATCH 06/31] mod: ArduinoAlvikWheel and ArduinoAlvikRgbLed as private classes mod: .begin() sleeps for 1s before returning mod: .stop() sets motors speed to 0 and turns off all LEDs mod: l_speed and r_speed removed feat: some unit of measurement conversion implemented upd: examples --- arduino_alvik.py | 70 ++++++++++++++++++++++++++++++-------- examples/message_reader.py | 8 ++--- examples/move_example.py | 8 ++--- examples/move_wheels.py | 36 ++++++++++++++++++++ robot_definitions.py | 1 + 5 files changed, 100 insertions(+), 23 deletions(-) create mode 100644 examples/move_wheels.py diff --git a/arduino_alvik.py b/arduino_alvik.py index 8dfba3f..efa0368 100644 --- a/arduino_alvik.py +++ b/arduino_alvik.py @@ -1,3 +1,5 @@ +import math + from uart import uart import _thread from time import sleep_ms @@ -13,15 +15,15 @@ class ArduinoAlvik: def __init__(self): self.packeter = ucPack(200) - self.left_wheel = ArduinoAlvikWheel(self.packeter, ord('L')) - self.right_wheel = ArduinoAlvikWheel(self.packeter, ord('R')) + 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, rgb_mask=[0b00100000, 0b01000000, 0b10000000]) + 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, + rgb_mask=[0b00100000, 0b01000000, 0b10000000]) self._update_thread_running = False self._update_thread_id = None - self.l_speed = None - self.r_speed = None self.battery_perc = None self.touch_bits = None self.behaviour = None @@ -50,6 +52,7 @@ def begin(self) -> int: self._begin_update_thread() sleep_ms(100) self._reset_hw() + sleep_ms(1000) return 0 def _begin_update_thread(self): @@ -73,7 +76,12 @@ def stop(self): :return: """ # stop engines + self.set_wheels_speed(0, 0) + # turn off UI leds + self._set_leds(0x00) + + # stop the update thrad self._stop_update_thread() @staticmethod @@ -88,16 +96,24 @@ def _reset_hw(): RESET_STM32.value(1) sleep_ms(100) - def get_speeds(self) -> (float, float): - return self.l_speed, self.r_speed + def get_wheels_speed(self) -> (float, float): + return self.left_wheel.get_speed(), self.right_wheel.get_speed() - def set_speeds(self, left_speed: float, right_speed: float): + def set_wheels_speed(self, left_speed: float, right_speed: float, unit: str = 'rpm'): """ Sets left/right motor speed :param left_speed: :param right_speed: + :param unit: the speed unit of measurement (default: 'rpm') :return: """ + + if unit not in angular_velocity_units: + return + elif unit == '%': + left_speed = perc_to_rpm(left_speed) + right_speed = perc_to_rpm(right_speed) + self.packeter.packetC2F(ord('J'), left_speed, right_speed) uart.write(self.packeter.msg[0:self.packeter.msg_size]) @@ -243,9 +259,7 @@ def _parse_message(self) -> int: code = self.packeter.payload[0] if code == ord('j'): # joint speed - _, self.l_speed, self.r_speed = self.packeter.unpacketC2F() - self.left_wheel._speed = self.l_speed - self.right_wheel._speed = self.r_speed + _, 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() @@ -333,7 +347,7 @@ def print_status(self): print(f'{str(a).upper()} = {getattr(self, str(a))}') -class ArduinoAlvikWheel: +class _ArduinoAlvikWheel: def __init__(self, packeter: ucPack, label: int, wheel_diameter_mm: float = WHEEL_DIAMETER_MM): self._packeter = packeter @@ -371,6 +385,11 @@ def set_speed(self, velocity: float, unit: str = 'rpm'): :return: """ + if unit not in angular_velocity_units: + return + elif unit == '%': + velocity = perc_to_rpm(velocity) + self._packeter.packetC2B1F(ord('W'), self._label & 0xFF, ord('V'), velocity) uart.write(self._packeter.msg[0:self._packeter.msg_size]) @@ -388,12 +407,18 @@ def set_position(self, position: float, unit: str = 'deg'): :param unit: the unit of measurement :return: """ + + if unit not in angle_units: + return + elif unit == 'rad': + position = rad_to_deg(position) + self._packeter.packetC2B1F(ord('W'), self._label & 0xFF, ord('P'), position) uart.write(self._packeter.msg[0:self._packeter.msg_size]) -class ArduinoAlvikRgbLed: - def __init__(self, packeter: ucPack, label: str, led_state: list[int], rgb_mask: list[int]): +class _ArduinoAlvikRgbLed: + def __init__(self, packeter: ucPack, label: str, led_state: list[int | None], rgb_mask: list[int]): self._packeter = packeter self.label = label self._rgb_mask = rgb_mask @@ -416,3 +441,18 @@ def set_color(self, red: bool, green: bool, blue: bool): self._led_state[0] = led_status self._packeter.packetC1B(ord('L'), led_status & 0xFF) uart.write(self._packeter.msg[0:self._packeter.msg_size]) + + +# Units and unit conversion methods + +angular_velocity_units = ['rpm', '%'] +angle_units = ['deg', 'rad'] +distance_units = ['mm', 'cm'] + + +def perc_to_rpm(percent: float) -> float: + return (percent / 100.0)*MOTOR_MAX_RPM + + +def rad_to_deg(rad: float) -> float: + return rad*180/math.pi diff --git a/examples/message_reader.py b/examples/message_reader.py index 219d226..ac5b49b 100644 --- a/examples/message_reader.py +++ b/examples/message_reader.py @@ -11,17 +11,17 @@ while True: try: print(f'VER: {alvik.version}') - print(f'LSP: {alvik.l_speed}') - print(f'RSP: {alvik.r_speed}') + print(f'LSP: {alvik.left_wheel.get_speed()}') + print(f'RSP: {alvik.left_wheel.get_speed()}') 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}') - alvik.set_speeds(speed, speed) + alvik.set_wheels_speed(speed, speed) speed = (speed + 1) % 60 sleep_ms(1000) except KeyboardInterrupt as e: print('over') - alvik.set_speeds(0, 0) + alvik.stop() break sys.exit() diff --git a/examples/move_example.py b/examples/move_example.py index fcb0a62..18c9e46 100644 --- a/examples/move_example.py +++ b/examples/move_example.py @@ -7,15 +7,15 @@ while True: try: - alvik.set_speeds(10, 10) + alvik.set_wheels_speed(10, 10) sleep_ms(1000) - alvik.set_speeds(30, 60) + alvik.set_wheels_speed(30, 60) sleep_ms(1000) - alvik.set_speeds(60, 30) + alvik.set_wheels_speed(60, 30) sleep_ms(1000) except KeyboardInterrupt as e: print('over') - alvik.set_speeds(0, 0) + alvik.stop() sys.exit() diff --git a/examples/move_wheels.py b/examples/move_wheels.py new file mode 100644 index 0000000..d6f6e7e --- /dev/null +++ b/examples/move_wheels.py @@ -0,0 +1,36 @@ +from arduino_alvik import ArduinoAlvik +from time import sleep_ms +import sys + +alvik = ArduinoAlvik() +alvik.begin() + +while True: + try: + alvik.left_wheel.set_speed(10) + sleep_ms(1000) + print(f'LSP: {alvik.left_wheel.get_speed()}') + print(f'RSP: {alvik.right_wheel.get_speed()}') + + alvik.right_wheel.set_speed(10) + sleep_ms(1000) + + print(f'LSP: {alvik.left_wheel.get_speed()}') + print(f'RSP: {alvik.right_wheel.get_speed()}') + + alvik.left_wheel.set_speed(20) + sleep_ms(1000) + + print(f'LSP: {alvik.left_wheel.get_speed()}') + print(f'RSP: {alvik.right_wheel.get_speed()}') + + alvik.right_wheel.set_speed(20) + sleep_ms(1000) + + print(f'LSP: {alvik.left_wheel.get_speed()}') + print(f'RSP: {alvik.right_wheel.get_speed()}') + + except KeyboardInterrupt as e: + print('over') + alvik.stop() + sys.exit() diff --git a/robot_definitions.py b/robot_definitions.py index f48df90..610378e 100644 --- a/robot_definitions.py +++ b/robot_definitions.py @@ -2,6 +2,7 @@ MOTOR_KP_DEFAULT = 32.0 MOTOR_KI_DEFAULT = 450.0 MOTOR_KD_DEFAULT = 0.0 +MOTOR_MAX_RPM = 70 # Wheels parameters WHEEL_DIAMETER_MM = 34 From baedaa01a8aef6fa79487e02757f67b9a6718a98 Mon Sep 17 00:00:00 2001 From: Lucio Rossi Date: Tue, 23 Jan 2024 17:45:09 +0100 Subject: [PATCH 07/31] feat: .drive method --- arduino_alvik.py | 10 ++++++++++ 1 file changed, 10 insertions(+) diff --git a/arduino_alvik.py b/arduino_alvik.py index efa0368..c84de68 100644 --- a/arduino_alvik.py +++ b/arduino_alvik.py @@ -146,6 +146,16 @@ def get_line_sensors(self) -> (int, int, int): return self.left_line, self.center_line, self.right_line + def drive(self, linear_velocity: float, angular_velocity: float): + """ + Drives the robot by linear and angular velocity + :param linear_velocity: + :param angular_velocity: + :return: + """ + self.packeter.packetC2F(ord('V'), linear_velocity, angular_velocity) + uart.write(self.packeter.msg[0:self.packeter.msg_size]) + def set_servo_positions(self, a_position: int, b_position: int): """ Sets A/B servomotor angle From bb432235790777405c0224ddffb8f3f2eb8bc845 Mon Sep 17 00:00:00 2001 From: Lucio Rossi Date: Tue, 23 Jan 2024 18:25:55 +0100 Subject: [PATCH 08/31] documentation --- arduino_alvik.py | 85 ++++++++++++++++++++++++++++++++++++++++++++---- 1 file changed, 78 insertions(+), 7 deletions(-) diff --git a/arduino_alvik.py b/arduino_alvik.py index c84de68..90aedb5 100644 --- a/arduino_alvik.py +++ b/arduino_alvik.py @@ -46,6 +46,10 @@ def __init__(self): self.version = [None, None, None] def begin(self) -> int: + """ + Begins all Alvik operations + :return: + """ if not CHECK_STM32.value(): print("\nTurn on your Arduino Alvik!\n") return -1 @@ -97,6 +101,10 @@ def _reset_hw(): sleep_ms(100) def get_wheels_speed(self) -> (float, float): + """ + Returns the speed of the wheels + :return: left_wheel_speed, right_wheel_speed + """ return self.left_wheel.get_speed(), self.right_wheel.get_speed() def set_wheels_speed(self, left_speed: float, right_speed: float, unit: str = 'rpm'): @@ -133,7 +141,7 @@ def set_pid(self, side: str, kp: float, ki: float, kd: float): def get_orientation(self) -> (float, float, float): """ Returns the orientation of the IMU - :return: + :return: roll, pitch, yaw """ return self.roll, self.pitch, self.yaw @@ -141,7 +149,7 @@ def get_orientation(self) -> (float, float, float): def get_line_sensors(self) -> (int, int, int): """ Returns the line sensors readout - :return: + :return: left_line, center_line, right_line """ return self.left_line, self.center_line, self.right_line @@ -307,36 +315,72 @@ def _parse_message(self) -> int: return 0 def _get_touch(self) -> int: + """ + Returns the touch sensor's state + :return: touch_bits + """ return self.touch_bits def get_touch_any(self) -> bool: + """ + Returns true if any button is pressed + :return: + """ 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) def get_touch_cancel(self) -> bool: + """ + Returns true if cancel button is pressed + :return: + """ 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) def get_touch_up(self) -> bool: + """ + Returns true if up button is pressed + :return: + """ 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) def get_touch_down(self) -> bool: + """ + Returns true if down button is pressed + :return: + """ 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) def get_color(self) -> (int, int, int): """ - Returns the RGB color readout - :return: + Returns the RGB color (raw) readout + :return: red, green, blue """ return self.red, self.green, self.blue @@ -345,12 +389,24 @@ def get_color(self) -> (int, int, int): # int((self.blue/COLOR_FULL_SCALE)*255)) def get_distance(self) -> (int, int, int, int, int, int): + """ + Returns the distance readout of the TOF sensor + :return: left_tof, center_left_tof, center_tof, center_right_tof, right_tof + """ return self.left_tof, self.center_left_tof, self.center_tof, self.center_right_tof, self.right_tof def get_version(self) -> str: + """ + Returns the firmware version of the Alvik + :return: + """ return f'{self.version[0]}.{self.version[1]}.{self.version[2]}' def print_status(self): + """ + Prints the Alvik status + :return: + """ for a in vars(self): if str(a).startswith('_'): continue @@ -366,6 +422,11 @@ def __init__(self, packeter: ucPack, label: int, wheel_diameter_mm: float = WHEE self._speed = None def reset(self, initial_position: float = 0.0): + """ + Resets the wheel reference position + :param initial_position: + :return: + """ pass def set_pid_gains(self, kp: float = MOTOR_KP_DEFAULT, ki: float = MOTOR_KI_DEFAULT, kd: float = MOTOR_KD_DEFAULT): @@ -389,7 +450,7 @@ def stop(self): def set_speed(self, velocity: float, unit: str = 'rpm'): """ - Sets left/right motor speed + Sets the motor speed :param velocity: the speed of the motor :param unit: the unit of measurement :return: @@ -403,9 +464,9 @@ def set_speed(self, velocity: float, unit: str = '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): + def get_speed(self) -> float: """ - Gets the current RPM speed of the wheel + Returns the current RPM speed of the wheel :return: """ return self._speed @@ -461,8 +522,18 @@ def set_color(self, red: bool, green: bool, blue: bool): def perc_to_rpm(percent: float) -> float: + """ + Converts percent of max_rpm to rpm + :param percent: + :return: + """ return (percent / 100.0)*MOTOR_MAX_RPM def rad_to_deg(rad: float) -> float: + """ + Converts radians to degrees + :param rad: + :return: + """ return rad*180/math.pi From bfbbffdd669773b9d877afdb2007f8febf5b1464 Mon Sep 17 00:00:00 2001 From: Lucio Rossi Date: Wed, 24 Jan 2024 15:11:23 +0100 Subject: [PATCH 09/31] mod: rem set_right_led and set_left_led color --- arduino_alvik.py | 30 ---------------- examples/led_setting.py | 36 ------------------- .../{rgb_leds_setting.py => leds_setting.py} | 0 3 files changed, 66 deletions(-) delete mode 100644 examples/led_setting.py rename examples/{rgb_leds_setting.py => leds_setting.py} (100%) diff --git a/arduino_alvik.py b/arduino_alvik.py index 90aedb5..eb59935 100644 --- a/arduino_alvik.py +++ b/arduino_alvik.py @@ -211,36 +211,6 @@ def set_illuminator(self, value: bool): 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 set_left_led_color(self, red: bool, green: bool, blue: bool): - """ - Sets the r,g,b state of the left LED - :param red: - :param green: - :param blue: - :return: - """ - if self.led_state[0] is None: - self._set_leds(0x00) - self.led_state[0] = self.led_state[0] | 0b00000100 if red else self.led_state[0] & 0b11111011 - self.led_state[0] = self.led_state[0] | 0b00001000 if green else self.led_state[0] & 0b11110111 - self.led_state[0] = self.led_state[0] | 0b00010000 if blue else self.led_state[0] & 0b11101111 - self._set_leds(self.led_state[0]) - - def set_right_led_color(self, red: bool, green: bool, blue: bool): - """ - Sets the r,g,b state of the right LED - :param red: - :param green: - :param blue: - :return: - """ - if self.led_state[0] is None: - self._set_leds(0x00) - self.led_state[0] = self.led_state[0] | 0b00100000 if red else self.led_state[0] & 0b11011111 - self.led_state[0] = self.led_state[0] | 0b01000000 if green else self.led_state[0] & 0b10111111 - self.led_state[0] = self.led_state[0] | 0b10000000 if blue else self.led_state[0] & 0b01111111 - self._set_leds(self.led_state[0]) - def _update(self, delay_=1): """ Updates the robot status reading/parsing messages from UART. diff --git a/examples/led_setting.py b/examples/led_setting.py deleted file mode 100644 index ac73241..0000000 --- a/examples/led_setting.py +++ /dev/null @@ -1,36 +0,0 @@ -from arduino_alvik import ArduinoAlvik -from time import sleep_ms -import sys - -alvik = ArduinoAlvik() -alvik.begin() - -while True: - try: - #alvik._set_leds(0xff) - #sleep_ms(1000) - #alvik._set_leds(0x00) - #sleep_ms(1000) - alvik.set_builtin_led(1) - sleep_ms(1000) - alvik.set_illuminator(1) - sleep_ms(1000) - alvik.set_builtin_led(0) - sleep_ms(1000) - alvik.set_illuminator(0) - sleep_ms(1000) - alvik.set_left_led_color(0,0,1) - sleep_ms(1000) - alvik.set_right_led_color(0,0,1) - sleep_ms(1000) - alvik.set_left_led_color(0,1,0) - sleep_ms(1000) - alvik.set_right_led_color(0,1,0) - sleep_ms(1000) - alvik.set_left_led_color(1,0,0) - sleep_ms(1000) - alvik.set_right_led_color(1,0,0) - sleep_ms(1000) - except KeyboardInterrupt as e: - print('over') - sys.exit() diff --git a/examples/rgb_leds_setting.py b/examples/leds_setting.py similarity index 100% rename from examples/rgb_leds_setting.py rename to examples/leds_setting.py From b364630c5e66ccaea87aa29086f37d22b7f43a62 Mon Sep 17 00:00:00 2001 From: Lucio Rossi Date: Wed, 24 Jan 2024 16:52:25 +0100 Subject: [PATCH 10/31] fix: thread must be singleton --- arduino_alvik.py | 12 ++++++++---- examples/message_reader.py | 4 ++-- 2 files changed, 10 insertions(+), 6 deletions(-) diff --git a/arduino_alvik.py b/arduino_alvik.py index eb59935..795062d 100644 --- a/arduino_alvik.py +++ b/arduino_alvik.py @@ -13,6 +13,8 @@ class ArduinoAlvik: + _update_thread_running = False + def __init__(self): self.packeter = ucPack(200) self.left_wheel = _ArduinoAlvikWheel(self.packeter, ord('L')) @@ -64,15 +66,17 @@ def _begin_update_thread(self): Runs robot background operations (e.g. threaded update) :return: """ - self._update_thread_running = True - self._update_thread_id = _thread.start_new_thread(self._update, (1,)) + + if not ArduinoAlvik._update_thread_running: + ArduinoAlvik._update_thread_running = True + self._update_thread_id = _thread.start_new_thread(self._update, (1,)) def _stop_update_thread(self): """ Stops the background operations :return: """ - self._update_thread_running = False + ArduinoAlvik._update_thread_running = False def stop(self): """ @@ -220,7 +224,7 @@ def _update(self, delay_=1): :return: """ while True: - if not self._update_thread_running: + if not ArduinoAlvik._update_thread_running: break if self._read_message(): self._parse_message() diff --git a/examples/message_reader.py b/examples/message_reader.py index ac5b49b..dae58ef 100644 --- a/examples/message_reader.py +++ b/examples/message_reader.py @@ -23,5 +23,5 @@ except KeyboardInterrupt as e: print('over') alvik.stop() - break -sys.exit() + sys.exit() + From 6d72cfa8eb1758938c2851aeb31da1d361ffa30f Mon Sep 17 00:00:00 2001 From: Lucio Rossi Date: Wed, 24 Jan 2024 16:55:51 +0100 Subject: [PATCH 11/31] fix: _stop_update_thread as staticmethod --- arduino_alvik.py | 3 ++- 1 file changed, 2 insertions(+), 1 deletion(-) diff --git a/arduino_alvik.py b/arduino_alvik.py index 795062d..7cb1484 100644 --- a/arduino_alvik.py +++ b/arduino_alvik.py @@ -71,7 +71,8 @@ def _begin_update_thread(self): ArduinoAlvik._update_thread_running = True self._update_thread_id = _thread.start_new_thread(self._update, (1,)) - def _stop_update_thread(self): + @staticmethod + def _stop_update_thread(): """ Stops the background operations :return: From 16b86292dc18b9c0003ee2b79e0510cb3c41c6eb Mon Sep 17 00:00:00 2001 From: Lucio Rossi Date: Wed, 24 Jan 2024 17:40:40 +0100 Subject: [PATCH 12/31] fix: some examples not calling alvik.stop() before sys.exit --- examples/leds_setting.py | 1 + examples/read_color_sensor.py | 4 ++-- examples/read_touch.py | 4 ++-- examples/set_pid.py | 4 ++-- 4 files changed, 7 insertions(+), 6 deletions(-) diff --git a/examples/leds_setting.py b/examples/leds_setting.py index 07c5781..9c1a462 100644 --- a/examples/leds_setting.py +++ b/examples/leds_setting.py @@ -37,4 +37,5 @@ sleep_ms(1000) except KeyboardInterrupt as e: print('over') + alvik.stop() sys.exit() diff --git a/examples/read_color_sensor.py b/examples/read_color_sensor.py index 5bf597c..3fafa12 100644 --- a/examples/read_color_sensor.py +++ b/examples/read_color_sensor.py @@ -13,5 +13,5 @@ sleep_ms(100) except KeyboardInterrupt as e: print('over') - break -sys.exit() + alvik.stop() + sys.exit() diff --git a/examples/read_touch.py b/examples/read_touch.py index abc0d5c..9f7e183 100644 --- a/examples/read_touch.py +++ b/examples/read_touch.py @@ -27,5 +27,5 @@ sleep_ms(100) except KeyboardInterrupt as e: print('over') - break -sys.exit() + alvik.stop() + sys.exit() diff --git a/examples/set_pid.py b/examples/set_pid.py index ad480f3..f59f6f4 100644 --- a/examples/set_pid.py +++ b/examples/set_pid.py @@ -14,5 +14,5 @@ sleep_ms(100) except KeyboardInterrupt as e: print('over') - break -sys.exit() + alvik.stop() + sys.exit() From 2314969eaffaa623dd67aa615b24c80f1260e370 Mon Sep 17 00:00:00 2001 From: Lucio Rossi Date: Fri, 26 Jan 2024 14:59:39 +0100 Subject: [PATCH 13/31] feat: ArduinoAlvik as singleton class fix: _update_thread_id should be class param fix: removed instance vars _update_thread_running and _update_thread_id mod: _stop_update_thread is a classmethod --- arduino_alvik.py | 20 ++++++++++++-------- 1 file changed, 12 insertions(+), 8 deletions(-) diff --git a/arduino_alvik.py b/arduino_alvik.py index 7cb1484..de670ae 100644 --- a/arduino_alvik.py +++ b/arduino_alvik.py @@ -14,6 +14,12 @@ class ArduinoAlvik: _update_thread_running = False + _update_thread_id = None + + def __new__(cls): + if not hasattr(cls, 'instance'): + cls.instance = super(ArduinoAlvik, cls).__new__(cls) + return cls.instance def __init__(self): self.packeter = ucPack(200) @@ -24,8 +30,6 @@ def __init__(self): rgb_mask=[0b00000100, 0b00001000, 0b00010000]) self.right_led = _ArduinoAlvikRgbLed(self.packeter, 'right', self.led_state, rgb_mask=[0b00100000, 0b01000000, 0b10000000]) - self._update_thread_running = False - self._update_thread_id = None self.battery_perc = None self.touch_bits = None self.behaviour = None @@ -67,17 +71,17 @@ def _begin_update_thread(self): :return: """ - if not ArduinoAlvik._update_thread_running: - ArduinoAlvik._update_thread_running = True - self._update_thread_id = _thread.start_new_thread(self._update, (1,)) + if not self.__class__._update_thread_running: + self.__class__._update_thread_running = True + self.__class__._update_thread_id = _thread.start_new_thread(self._update, (1,)) - @staticmethod - def _stop_update_thread(): + @classmethod + def _stop_update_thread(cls): """ Stops the background operations :return: """ - ArduinoAlvik._update_thread_running = False + cls._update_thread_running = False def stop(self): """ From 15580df0f0aed568630619549b84241cdb7f8681 Mon Sep 17 00:00:00 2001 From: Lucio Rossi Date: Fri, 26 Jan 2024 15:32:25 +0100 Subject: [PATCH 14/31] feat: reset/update/get wheels position wheels_servo.py --- arduino_alvik.py | 16 ++++++++++++++-- examples/wheels_servo.py | 36 ++++++++++++++++++++++++++++++++++++ 2 files changed, 50 insertions(+), 2 deletions(-) create mode 100644 examples/wheels_servo.py diff --git a/arduino_alvik.py b/arduino_alvik.py index eb59935..d35213c 100644 --- a/arduino_alvik.py +++ b/arduino_alvik.py @@ -276,6 +276,9 @@ def _parse_message(self) -> int: elif code == ord('q'): # imu position _, self.roll, self.pitch, self.yaw = self.packeter.unpacketC3F() + if code == ord('w'): + # wheels position + _, self.left_wheel._position, self.right_wheel._position = self.packeter.unpacketC2F() elif code == 0x7E: # firmware version _, *self.version = self.packeter.unpacketC3B() @@ -390,6 +393,7 @@ def __init__(self, packeter: ucPack, label: int, wheel_diameter_mm: float = WHEE self._label = label self._wheel_diameter_mm = wheel_diameter_mm self._speed = None + self._position = None def reset(self, initial_position: float = 0.0): """ @@ -397,7 +401,8 @@ def reset(self, initial_position: float = 0.0): :param initial_position: :return: """ - pass + self._packeter.packetC2B1F(ord('W'), self._label & 0xFF, ord('Z'), initial_position) + uart.write(self._packeter.msg[0:self._packeter.msg_size]) def set_pid_gains(self, kp: float = MOTOR_KP_DEFAULT, ki: float = MOTOR_KI_DEFAULT, kd: float = MOTOR_KD_DEFAULT): """ @@ -408,7 +413,7 @@ def set_pid_gains(self, kp: float = MOTOR_KP_DEFAULT, ki: float = MOTOR_KI_DEFAU :return: """ - self._packeter.packetC1B3F(ord('P'), self._label, kp, ki, kd) + self._packeter.packetC1B3F(ord('P'), self._label & 0xFF, kp, ki, kd) uart.write(self._packeter.msg[0:self._packeter.msg_size]) def stop(self): @@ -441,6 +446,13 @@ def get_speed(self) -> float: """ return self._speed + def get_position(self) -> float: + """ + Returns the wheel position (angle with respect to the reference) + :return: + """ + return self._position + def set_position(self, position: float, unit: str = 'deg'): """ Sets left/right motor speed diff --git a/examples/wheels_servo.py b/examples/wheels_servo.py new file mode 100644 index 0000000..b4220dd --- /dev/null +++ b/examples/wheels_servo.py @@ -0,0 +1,36 @@ +from arduino_alvik import ArduinoAlvik +from time import sleep +import sys + +alvik = ArduinoAlvik() +alvik.begin() + +alvik.left_wheel.reset() +alvik.right_wheel.reset() + +while True: + try: + alvik.left_wheel.set_position(30) + sleep(2) + print(f'Left wheel degs: {alvik.left_wheel.get_position()}') + print(f'Right wheel degs: {alvik.right_wheel.get_position()}') + + alvik.right_wheel.set_position(10) + sleep(2) + print(f'Left wheel degs: {alvik.left_wheel.get_position()}') + print(f'Right wheel degs: {alvik.right_wheel.get_position()}') + + alvik.left_wheel.set_position(180) + sleep(2) + print(f'Left wheel degs: {alvik.left_wheel.get_position()}') + print(f'Right wheel degs: {alvik.right_wheel.get_position()}') + + alvik.right_wheel.set_position(270) + sleep(2) + print(f'Left wheel degs: {alvik.left_wheel.get_position()}') + print(f'Right wheel degs: {alvik.right_wheel.get_position()}') + + except KeyboardInterrupt as e: + print('over') + alvik.stop() + sys.exit() From 92405850bfc28964e2beb5d06fa47f9dba2cc6f7 Mon Sep 17 00:00:00 2001 From: Lucio Rossi Date: Fri, 26 Jan 2024 15:39:41 +0100 Subject: [PATCH 15/31] message_reader.py fix: reading wrong wheel add: wheels position --- examples/message_reader.py | 4 +++- 1 file changed, 3 insertions(+), 1 deletion(-) diff --git a/examples/message_reader.py b/examples/message_reader.py index ac5b49b..2d367f9 100644 --- a/examples/message_reader.py +++ b/examples/message_reader.py @@ -12,7 +12,9 @@ try: print(f'VER: {alvik.version}') print(f'LSP: {alvik.left_wheel.get_speed()}') - print(f'RSP: {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: {alvik.touch_bits}') print(f'RGB: {alvik.red} {alvik.green} {alvik.blue}') print(f'LINE: {alvik.left_line} {alvik.center_line} {alvik.right_line}') From b94c5f79f6a408e83452874f92b88ca18fd9ee08 Mon Sep 17 00:00:00 2001 From: Lucio Rossi Date: Fri, 26 Jan 2024 15:42:27 +0100 Subject: [PATCH 16/31] mod: begin turn on illuminator --- arduino_alvik.py | 1 + 1 file changed, 1 insertion(+) diff --git a/arduino_alvik.py b/arduino_alvik.py index d35213c..c52a611 100644 --- a/arduino_alvik.py +++ b/arduino_alvik.py @@ -57,6 +57,7 @@ def begin(self) -> int: sleep_ms(100) self._reset_hw() sleep_ms(1000) + self.set_illuminator(True) return 0 def _begin_update_thread(self): From a849833a45c518e1d56caeaac313c8e8ce65d36c Mon Sep 17 00:00:00 2001 From: Lucio Rossi Date: Tue, 30 Jan 2024 12:15:03 +0100 Subject: [PATCH 17/31] fix: install scripts missing cp robot_definitions.py --- install.bat | 2 ++ install.sh | 2 ++ 2 files changed, 4 insertions(+) diff --git a/install.bat b/install.bat index e4b41d1..7871cc1 100644 --- a/install.bat +++ b/install.bat @@ -19,11 +19,13 @@ if /i "%1"=="-h" ( python -m mpremote %port_string% fs rm :arduino_alvik.py python -m mpremote %port_string% fs rm :constants.py python -m mpremote %port_string% fs rm :pinout_definitions.py +python -m mpremote %port_string% fs rm :robot_definitions.py python -m mpremote %port_string% fs rm :uart.py python -m mpremote %port_string% fs cp arduino_alvik.py :arduino_alvik.py python -m mpremote %port_string% fs cp constants.py :constants.py python -m mpremote %port_string% fs cp pinout_definitions.py :pinout_definitions.py +python -m mpremote %port_string% fs cp robot_definitions.py :robot_definitions.py python -m mpremote %port_string% fs cp uart.py :uart.py echo Installing dependencies diff --git a/install.sh b/install.sh index 3e1821e..8129e2c 100644 --- a/install.sh +++ b/install.sh @@ -45,11 +45,13 @@ fi $python_command -m mpremote $connect_string fs rm :arduino_alvik.py $python_command -m mpremote $connect_string fs rm :constants.py $python_command -m mpremote $connect_string fs rm :pinout_definitions.py +$python_command -m mpremote $connect_string fs rm :robot_definitions.py $python_command -m mpremote $connect_string fs rm :uart.py $python_command -m mpremote $connect_string fs cp arduino_alvik.py :arduino_alvik.py $python_command -m mpremote $connect_string fs cp constants.py :constants.py $python_command -m mpremote $connect_string fs cp pinout_definitions.py :pinout_definitions.py +$python_command -m mpremote $connect_string fs cp robot_definitions.py :robot_definitions.py $python_command -m mpremote $connect_string fs cp uart.py :uart.py echo "Installing dependencies" From db5e9f2b89ccb7be3fbee9d1dfa4f1a3b9d70b88 Mon Sep 17 00:00:00 2001 From: Lucio Rossi Date: Tue, 30 Jan 2024 14:59:27 +0100 Subject: [PATCH 18/31] feat: get_drive_speed --- arduino_alvik.py | 14 +++++++++++++- 1 file changed, 13 insertions(+), 1 deletion(-) diff --git a/arduino_alvik.py b/arduino_alvik.py index 3d4d56c..0d13a6c 100644 --- a/arduino_alvik.py +++ b/arduino_alvik.py @@ -49,6 +49,8 @@ def __init__(self): self.right_tof = None self.top_tof = None self.bottom_tof = None + self.linear_velocity = None + self.angular_velocity = None self.version = [None, None, None] def begin(self) -> int: @@ -174,6 +176,13 @@ def drive(self, linear_velocity: float, angular_velocity: float): self.packeter.packetC2F(ord('V'), linear_velocity, angular_velocity) uart.write(self.packeter.msg[0:self.packeter.msg_size]) + def get_drive_speed(self) -> (float, float): + """ + Returns linear and angular velocity of the robot + :return: linear_velocity, angular_velocity + """ + return self.linear_velocity, self.angular_velocity + def set_servo_positions(self, a_position: int, b_position: int): """ Sets A/B servomotor angle @@ -286,9 +295,12 @@ def _parse_message(self) -> int: elif code == ord('q'): # imu position _, self.roll, self.pitch, self.yaw = self.packeter.unpacketC3F() - if code == ord('w'): + elif code == ord('w'): # wheels position _, 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() elif code == 0x7E: # firmware version _, *self.version = self.packeter.unpacketC3B() From f0e1126f8cb53e768c4b9d1688de0dd1de70610a Mon Sep 17 00:00:00 2001 From: Lucio Rossi Date: Tue, 30 Jan 2024 18:30:25 +0100 Subject: [PATCH 19/31] feat: move and rotate --- arduino_alvik.py | 29 +++++++++++++++++++++++++++++ examples/move_example.py | 16 ++++++++++------ examples/wheels_speed.py | 21 +++++++++++++++++++++ 3 files changed, 60 insertions(+), 6 deletions(-) create mode 100644 examples/wheels_speed.py diff --git a/arduino_alvik.py b/arduino_alvik.py index 0d13a6c..acd0d90 100644 --- a/arduino_alvik.py +++ b/arduino_alvik.py @@ -51,6 +51,7 @@ def __init__(self): self.bottom_tof = None self.linear_velocity = None self.angular_velocity = None + self.last_ack = '' self.version = [None, None, None] def begin(self) -> int: @@ -86,6 +87,24 @@ def _stop_update_thread(cls): """ cls._update_thread_running = False + def rotate(self, angle: float): + """ + Rotates the robot by given angle + :param angle: + :return: + """ + self.packeter.packetC1F(ord('R'), angle) + uart.write(self.packeter.msg[0:self.packeter.msg_size]) + + def move(self, distance: float): + """ + Moves the robot by given distance + :param distance: + :return: + """ + self.packeter.packetC1F(ord('G'), distance) + uart.write(self.packeter.msg[0:self.packeter.msg_size]) + def stop(self): """ Stops all Alvik operations @@ -193,6 +212,13 @@ 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): + """ + Resets and returns last acknowledgement + :return: + """ + return self.last_ack + # def send_ack(self): # self.packeter.packetC1B(ord('X'), ACK_) # uart.write(self.packeter.msg[0:self.packeter.msg_size]) @@ -301,6 +327,9 @@ def _parse_message(self) -> int: elif code == ord('v'): # robot velocity _, self.linear_velocity, self.angular_velocity = self.packeter.unpacketC2F() + elif code == ord('x'): + # robot ack + _, self.last_ack = self.packeter.unpacketC1B() elif code == 0x7E: # firmware version _, *self.version = self.packeter.unpacketC3B() diff --git a/examples/move_example.py b/examples/move_example.py index 18c9e46..361e4ad 100644 --- a/examples/move_example.py +++ b/examples/move_example.py @@ -7,14 +7,18 @@ while True: try: - alvik.set_wheels_speed(10, 10) - sleep_ms(1000) + alvik.move(100.0) + while (ack := alvik.get_ack()) != ord('M'): + print(f'moving... not on target yet ack={ack}') + sleep_ms(200) + print("on target after move") - alvik.set_wheels_speed(30, 60) - sleep_ms(1000) + alvik.rotate(90.0) + while (ack := alvik.get_ack()) != ord('R'): + print(f'rotating... not on target yet ack={ack}') + sleep_ms(200) + print("on target after rotation") - alvik.set_wheels_speed(60, 30) - sleep_ms(1000) except KeyboardInterrupt as e: print('over') alvik.stop() diff --git a/examples/wheels_speed.py b/examples/wheels_speed.py new file mode 100644 index 0000000..18c9e46 --- /dev/null +++ b/examples/wheels_speed.py @@ -0,0 +1,21 @@ +from arduino_alvik import ArduinoAlvik +from time import sleep_ms +import sys + +alvik = ArduinoAlvik() +alvik.begin() + +while True: + try: + alvik.set_wheels_speed(10, 10) + sleep_ms(1000) + + alvik.set_wheels_speed(30, 60) + sleep_ms(1000) + + alvik.set_wheels_speed(60, 30) + sleep_ms(1000) + except KeyboardInterrupt as e: + print('over') + alvik.stop() + sys.exit() From fde782d78a148fffd4d4dbed8e5a6d056c33d96b Mon Sep 17 00:00:00 2001 From: Lucio Rossi Date: Wed, 31 Jan 2024 12:37:53 +0100 Subject: [PATCH 20/31] mod: remove alvik.set_pid delegate function to wheel objects - closes ALVIKDEV-37 --- arduino_alvik.py | 13 ------------- examples/set_pid.py | 4 ++-- 2 files changed, 2 insertions(+), 15 deletions(-) diff --git a/arduino_alvik.py b/arduino_alvik.py index acd0d90..2ee1dca 100644 --- a/arduino_alvik.py +++ b/arduino_alvik.py @@ -156,19 +156,6 @@ 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]) - def set_pid(self, side: str, kp: float, ki: float, kd: float): - """ - Sets motor PID parameters. Side can be 'L' or 'R' - :param side: - :param kp: - :param ki: - :param kd: - :return: - """ - - self.packeter.packetC1B3F(ord('P'), ord(side), kp, ki, kd) - uart.write(self.packeter.msg[0:self.packeter.msg_size]) - def get_orientation(self) -> (float, float, float): """ Returns the orientation of the IMU diff --git a/examples/set_pid.py b/examples/set_pid.py index f59f6f4..77909f9 100644 --- a/examples/set_pid.py +++ b/examples/set_pid.py @@ -8,9 +8,9 @@ while True: try: - alvik.set_pid('L', 10.0, 1.3, 4.2) + alvik.left_wheel.set_pid_gains(10.0, 1.3, 4.2) sleep_ms(100) - alvik.set_pid('R', 4.0, 13, 1.9) + alvik.right_wheel.set_pid_gains(4.0, 13, 1.9) sleep_ms(100) except KeyboardInterrupt as e: print('over') From db5cec26fdf45b42dc876d23590267bc00172f9e Mon Sep 17 00:00:00 2001 From: Lucio Rossi Date: Wed, 31 Jan 2024 12:43:08 +0100 Subject: [PATCH 21/31] mod: renames alvik.get_color to get_color_raw - closes ALVIKDEV-39 --- arduino_alvik.py | 4 ++-- examples/read_color_sensor.py | 2 +- 2 files changed, 3 insertions(+), 3 deletions(-) diff --git a/arduino_alvik.py b/arduino_alvik.py index 2ee1dca..c17797a 100644 --- a/arduino_alvik.py +++ b/arduino_alvik.py @@ -388,9 +388,9 @@ def get_touch_right(self) -> bool: """ return bool(self.touch_bits & 0b10000000) - def get_color(self) -> (int, int, int): + def get_color_raw(self) -> (int, int, int): """ - Returns the RGB color (raw) readout + Returns the color sensor's raw readout :return: red, green, blue """ diff --git a/examples/read_color_sensor.py b/examples/read_color_sensor.py index 3fafa12..27636b2 100644 --- a/examples/read_color_sensor.py +++ b/examples/read_color_sensor.py @@ -8,7 +8,7 @@ while True: try: - r, g, b = alvik.get_color() + r, g, b = alvik.get_color_raw() print(f'RED: {r}, Green: {g}, Blue: {b}') sleep_ms(100) except KeyboardInterrupt as e: From 5a35ada12773f435a5670cc69b6e688c684419a1 Mon Sep 17 00:00:00 2001 From: Lucio Rossi Date: Wed, 31 Jan 2024 13:08:59 +0100 Subject: [PATCH 22/31] feat: IMUs readouts - closes ALVIKDEV-22 --- arduino_alvik.py | 29 ++++++++++++++++++++++++++++- examples/read_imu.py | 18 ++++++++++++++++++ 2 files changed, 46 insertions(+), 1 deletion(-) create mode 100644 examples/read_imu.py diff --git a/arduino_alvik.py b/arduino_alvik.py index c17797a..49d3466 100644 --- a/arduino_alvik.py +++ b/arduino_alvik.py @@ -42,6 +42,12 @@ def __init__(self): self.roll = None self.pitch = None self.yaw = 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 @@ -164,6 +170,27 @@ def get_orientation(self) -> (float, float, float): 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 + + def get_gyro(self) -> (float, float, float): + """ + 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): + """ + 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): """ Returns the line sensors readout @@ -288,7 +315,7 @@ def _parse_message(self) -> int: _, self.red, self.green, self.blue = self.packeter.unpacketC3I() elif code == ord('i'): # imu - _, ax, ay, az, gx, gy, 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() diff --git a/examples/read_imu.py b/examples/read_imu.py new file mode 100644 index 0000000..cab0a90 --- /dev/null +++ b/examples/read_imu.py @@ -0,0 +1,18 @@ +from arduino_alvik import ArduinoAlvik +from time import sleep_ms +import sys + +alvik = ArduinoAlvik() +alvik.begin() +speed = 0 + +while True: + try: + ax, ay, az = alvik.get_accelerations() + gx, gy, gz = alvik.get_gyro() + print(f'ax: {ax}, ay: {ay}, az: {az}, gx: {gx}, gy: {gy}, gz: {gz}') + sleep_ms(100) + except KeyboardInterrupt as e: + print('over') + alvik.stop() + sys.exit() From be3fcad2847386bb6eb92bfeff53bb4ef65c549b Mon Sep 17 00:00:00 2001 From: Lucio Rossi Date: Wed, 31 Jan 2024 13:10:36 +0100 Subject: [PATCH 23/31] fix: previous typo --- arduino_alvik.py | 2 +- examples/read_imu.py | 2 +- 2 files changed, 2 insertions(+), 2 deletions(-) diff --git a/arduino_alvik.py b/arduino_alvik.py index 49d3466..a555f11 100644 --- a/arduino_alvik.py +++ b/arduino_alvik.py @@ -177,7 +177,7 @@ def get_accelerations(self) -> (float, float, float): """ return self.ax, self.ay, self.az - def get_gyro(self) -> (float, float, float): + def get_gyros(self) -> (float, float, float): """ Returns the 3-axial angular acceleration of the IMU :return: gx, gy, gz diff --git a/examples/read_imu.py b/examples/read_imu.py index cab0a90..9e82380 100644 --- a/examples/read_imu.py +++ b/examples/read_imu.py @@ -9,7 +9,7 @@ while True: try: ax, ay, az = alvik.get_accelerations() - gx, gy, gz = alvik.get_gyro() + gx, gy, gz = alvik.get_gyros() print(f'ax: {ax}, ay: {ay}, az: {az}, gx: {gx}, gy: {gy}, gz: {gz}') sleep_ms(100) except KeyboardInterrupt as e: From 48bd2952148f6282e633c9a763dbd9189c47f248 Mon Sep 17 00:00:00 2001 From: Lucio Rossi Date: Wed, 31 Jan 2024 16:29:55 +0100 Subject: [PATCH 24/31] feat: set_pose --- arduino_alvik.py | 11 +++++++++++ 1 file changed, 11 insertions(+) diff --git a/arduino_alvik.py b/arduino_alvik.py index a555f11..6788ecd 100644 --- a/arduino_alvik.py +++ b/arduino_alvik.py @@ -216,6 +216,17 @@ def get_drive_speed(self) -> (float, float): """ return self.linear_velocity, self.angular_velocity + def set_pose(self, x: float, y: float, theta: float): + """ + Sets the robot pose + :param x: x coordinate of the robot + :param y: y coordinate of the robot + :param theta: angle of the robot + :return: + """ + self.packeter.packetC3F(ord('Z'), x, y, theta) + uart.write(self.packeter.msg[0:self.packeter.msg_size]) + def set_servo_positions(self, a_position: int, b_position: int): """ Sets A/B servomotor angle From 72cd9c08aaaccfb5f5f25dd4c418a061891350a6 Mon Sep 17 00:00:00 2001 From: Lucio Rossi Date: Wed, 31 Jan 2024 16:32:11 +0100 Subject: [PATCH 25/31] feat: reset_pose amends previous --- arduino_alvik.py | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/arduino_alvik.py b/arduino_alvik.py index 6788ecd..e96733b 100644 --- a/arduino_alvik.py +++ b/arduino_alvik.py @@ -216,9 +216,9 @@ def get_drive_speed(self) -> (float, float): """ return self.linear_velocity, self.angular_velocity - def set_pose(self, x: float, y: float, theta: float): + def reset_pose(self, x: float, y: float, theta: float): """ - Sets the robot pose + Resets the robot pose :param x: x coordinate of the robot :param y: y coordinate of the robot :param theta: angle of the robot From 9a1cf19f169b9e145e3a915538a73467adeb92f7 Mon Sep 17 00:00:00 2001 From: Lucio Rossi Date: Wed, 31 Jan 2024 17:24:33 +0100 Subject: [PATCH 26/31] feat: get_pose --- arduino_alvik.py | 13 +++++++++++++ 1 file changed, 13 insertions(+) diff --git a/arduino_alvik.py b/arduino_alvik.py index e96733b..e91e0dd 100644 --- a/arduino_alvik.py +++ b/arduino_alvik.py @@ -42,6 +42,9 @@ def __init__(self): 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 @@ -227,6 +230,13 @@ def reset_pose(self, x: float, y: float, theta: float): self.packeter.packetC3F(ord('Z'), x, y, theta) uart.write(self.packeter.msg[0:self.packeter.msg_size]) + def get_pose(self) -> (float, float, float): + """ + Returns the current pose of the robot + :return: x, y, theta + """ + return self.x, self.y, self.theta + def set_servo_positions(self, a_position: int, b_position: int): """ Sets A/B servomotor angle @@ -355,6 +365,9 @@ def _parse_message(self) -> int: elif code == ord('x'): # robot ack _, self.last_ack = self.packeter.unpacketC1B() + elif code == ord('z'): + # robot ack + _, self.x, self.y, self.theta = self.packeter.unpacketC3F() elif code == 0x7E: # firmware version _, *self.version = self.packeter.unpacketC3B() From 7126cf0f8045cf09619eb8024f3032ed5e5cf24e Mon Sep 17 00:00:00 2001 From: Lucio Rossi Date: Wed, 31 Jan 2024 18:06:14 +0100 Subject: [PATCH 27/31] example: pose_example.py --- examples/pose_example.py | 43 ++++++++++++++++++++++++++++++++++++++++ 1 file changed, 43 insertions(+) create mode 100644 examples/pose_example.py diff --git a/examples/pose_example.py b/examples/pose_example.py new file mode 100644 index 0000000..9cb5d56 --- /dev/null +++ b/examples/pose_example.py @@ -0,0 +1,43 @@ +from arduino_alvik import ArduinoAlvik +from time import sleep_ms +import sys + +alvik = ArduinoAlvik() +alvik.begin() + +while True: + try: + + alvik.move(100.0) + while (ack := alvik.get_ack()) != ord('M'): + sleep_ms(200) + print("on target after move") + + alvik.rotate(90.0) + while (ack := alvik.get_ack()) != ord('R'): + sleep_ms(200) + print("on target after rotation") + + alvik.move(50.0) + while (ack := alvik.get_ack()) != ord('M'): + sleep_ms(200) + print("on target after move") + + alvik.rotate(-45.00) + while (ack := alvik.get_ack()) != ord('R'): + sleep_ms(200) + print("on target after rotation") + + x, y, theta = alvik.get_pose() + print(f'Current pose is x={x}, y={y} ,theta={theta}') + + alvik.reset_pose(0, 0, 0) + + x, y, theta = alvik.get_pose() + print(f'Updated pose is x={x}, y={y} ,theta={theta}') + sleep_ms(500) + + except KeyboardInterrupt as e: + print('over') + alvik.stop() + sys.exit() From bd1ec4dc4777457725a13d6aeb6bb2bcfd6659f6 Mon Sep 17 00:00:00 2001 From: Lucio Rossi Date: Fri, 2 Feb 2024 12:10:45 +0100 Subject: [PATCH 28/31] feat: delays on rotate move and reset_pose --- arduino_alvik.py | 3 +++ pinout_definitions.py | 10 +++++++++- 2 files changed, 12 insertions(+), 1 deletion(-) diff --git a/arduino_alvik.py b/arduino_alvik.py index e91e0dd..142eae4 100644 --- a/arduino_alvik.py +++ b/arduino_alvik.py @@ -102,6 +102,7 @@ def rotate(self, angle: float): :param angle: :return: """ + sleep_ms(200) self.packeter.packetC1F(ord('R'), angle) uart.write(self.packeter.msg[0:self.packeter.msg_size]) @@ -111,6 +112,7 @@ def move(self, distance: float): :param distance: :return: """ + sleep_ms(200) self.packeter.packetC1F(ord('G'), distance) uart.write(self.packeter.msg[0:self.packeter.msg_size]) @@ -229,6 +231,7 @@ def reset_pose(self, x: float, y: float, theta: float): """ 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) -> (float, float, float): """ diff --git a/pinout_definitions.py b/pinout_definitions.py index e9fe791..a24e3d9 100644 --- a/pinout_definitions.py +++ b/pinout_definitions.py @@ -3,7 +3,15 @@ # NANO to STM32 PINS D2 = 5 # ESP32 pin5 -> nano D2 D3 = 6 # ESP32 pin6 -> nano D3 +D4 = 7 # ESP32 pin7 -> nano D4 + +A4 = 11 # ESP32 pin11 SDA -> nano A4 +A5 = 12 # ESP32 pin12 SCL -> nano A5 A6 = 13 # ESP32 pin13 -> nano A6/D23 + BOOT0_STM32 = Pin(D2, Pin.OUT) # nano D2 -> STM32 Boot0 -RESET_STM32 = Pin(D3, Pin.OUT) # nano D2 -> STM32 NRST +RESET_STM32 = Pin(D3, Pin.OUT) # nano D3 -> STM32 NRST +NANO_CHK = Pin(D4, Pin.OUT) # nano D3 -> STM32 NRST CHECK_STM32 = Pin(A6, Pin.IN, Pin.PULL_UP) # nano A6/D23 -> STM32 ROBOT_CHK +ESP32_SDA = Pin(A4, Pin.OUT) +ESP32_SCL = Pin(A5, Pin.OUT) From ccb3dd4fa9668913f452129f84d30b6f5f6de083 Mon Sep 17 00:00:00 2001 From: Lucio Rossi Date: Fri, 2 Feb 2024 16:36:25 +0100 Subject: [PATCH 29/31] fix: begin clears uart and waits for robot comm fix: move and rotate wait on target feat: rotate and move blocking / non-blocking feat: wait_for_target, is_target_reached ex: pose_example.py blocking/non-blocking --- arduino_alvik.py | 34 +++++++++++++++++++++++++++-- examples/pose_example.py | 46 +++++++++++++++++++++++++++++++--------- 2 files changed, 68 insertions(+), 12 deletions(-) diff --git a/arduino_alvik.py b/arduino_alvik.py index 142eae4..633ec49 100644 --- a/arduino_alvik.py +++ b/arduino_alvik.py @@ -1,5 +1,7 @@ import math +import gc + from uart import uart import _thread from time import sleep_ms @@ -74,7 +76,11 @@ def begin(self) -> int: self._begin_update_thread() sleep_ms(100) self._reset_hw() + while uart.any(): + uart.read(1) sleep_ms(1000) + while self.last_ack != 0x00: + sleep_ms(20) self.set_illuminator(True) return 0 @@ -96,25 +102,45 @@ def _stop_update_thread(cls): """ cls._update_thread_running = False - def rotate(self, angle: float): + def _wait_for_target(self): + while not self.is_target_reached(): + pass + + def is_target_reached(self) -> bool: + if self.last_ack != ord('M') and self.last_ack != 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]) + sleep_ms(200) + return True + + def rotate(self, angle: float, blocking: bool = True): """ Rotates the robot by given angle :param angle: + :param blocking: :return: """ sleep_ms(200) self.packeter.packetC1F(ord('R'), angle) uart.write(self.packeter.msg[0:self.packeter.msg_size]) + if blocking: + self._wait_for_target() - def move(self, distance: float): + def move(self, distance: float, blocking: bool = True): """ Moves the robot by given distance :param distance: + :param blocking: :return: """ sleep_ms(200) self.packeter.packetC1F(ord('G'), distance) uart.write(self.packeter.msg[0:self.packeter.msg_size]) + if blocking: + self._wait_for_target() def stop(self): """ @@ -130,6 +156,10 @@ def stop(self): # stop the update thrad self._stop_update_thread() + # delete instance + del self.__class__.instance + gc.collect() + @staticmethod def _reset_hw(): """ diff --git a/examples/pose_example.py b/examples/pose_example.py index 9cb5d56..3f4f40e 100644 --- a/examples/pose_example.py +++ b/examples/pose_example.py @@ -9,23 +9,46 @@ try: alvik.move(100.0) - while (ack := alvik.get_ack()) != ord('M'): - sleep_ms(200) + print("on target after move") + + alvik.move(50.0) print("on target after move") alvik.rotate(90.0) - while (ack := alvik.get_ack()) != ord('R'): - sleep_ms(200) print("on target after rotation") - alvik.move(50.0) - while (ack := alvik.get_ack()) != ord('M'): - sleep_ms(200) + alvik.rotate(-45.00) + print("on target after rotation") + + x, y, theta = alvik.get_pose() + print(f'Current pose is x={x}, y={y} ,theta={theta}') + + alvik.reset_pose(0, 0, 0) + + x, y, theta = alvik.get_pose() + print(f'Updated pose is x={x}, y={y} ,theta={theta}') + sleep_ms(500) + + print("___________NON-BLOCKING__________________") + + alvik.move(50.0, blocking=False) + while not alvik.is_target_reached(): + print(f"Not yet on target received:{alvik.last_ack}") print("on target after move") - alvik.rotate(-45.00) - while (ack := alvik.get_ack()) != ord('R'): - sleep_ms(200) + alvik.rotate(45.0, blocking=False) + while not alvik.is_target_reached(): + print(f"Not yet on target received:{alvik.last_ack}") + print("on target after rotation") + + alvik.move(100.0, blocking=False) + while not alvik.is_target_reached(): + print(f"Not yet on target received:{alvik.last_ack}") + print("on target after move") + + alvik.rotate(-90.00, blocking=False) + while not alvik.is_target_reached(): + print(f"Not yet on target received:{alvik.last_ack}") print("on target after rotation") x, y, theta = alvik.get_pose() @@ -37,6 +60,9 @@ print(f'Updated pose is x={x}, y={y} ,theta={theta}') sleep_ms(500) + alvik.stop() + sys.exit() + except KeyboardInterrupt as e: print('over') alvik.stop() From 1d04c9989cf823bd898df98a2af987c192efd8bb Mon Sep 17 00:00:00 2001 From: Lucio Rossi Date: Fri, 2 Feb 2024 17:42:36 +0100 Subject: [PATCH 30/31] ver: 0.1.0 --- package.json | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/package.json b/package.json index 3944440..556f12b 100644 --- a/package.json +++ b/package.json @@ -3,5 +3,5 @@ ], "deps": [ ], - "version": "0.0.7" + "version": "0.1.0" } \ No newline at end of file From 6f491177270e5ad9540c033ec9bd87833da8b5e2 Mon Sep 17 00:00:00 2001 From: Lucio Rossi Date: Fri, 2 Feb 2024 17:52:41 +0100 Subject: [PATCH 31/31] examples revamp --- examples/leds_setting.py | 4 ---- examples/move_example.py | 25 ------------------------- 2 files changed, 29 deletions(-) delete mode 100644 examples/move_example.py diff --git a/examples/leds_setting.py b/examples/leds_setting.py index 9c1a462..cceb6c4 100644 --- a/examples/leds_setting.py +++ b/examples/leds_setting.py @@ -7,10 +7,6 @@ while True: try: - alvik._set_leds(0xff) - sleep_ms(1000) - alvik._set_leds(0x00) - sleep_ms(1000) alvik.set_builtin_led(1) sleep_ms(1000) alvik.set_illuminator(1) diff --git a/examples/move_example.py b/examples/move_example.py deleted file mode 100644 index 361e4ad..0000000 --- a/examples/move_example.py +++ /dev/null @@ -1,25 +0,0 @@ -from arduino_alvik import ArduinoAlvik -from time import sleep_ms -import sys - -alvik = ArduinoAlvik() -alvik.begin() - -while True: - try: - alvik.move(100.0) - while (ack := alvik.get_ack()) != ord('M'): - print(f'moving... not on target yet ack={ack}') - sleep_ms(200) - print("on target after move") - - alvik.rotate(90.0) - while (ack := alvik.get_ack()) != ord('R'): - print(f'rotating... not on target yet ack={ack}') - sleep_ms(200) - print("on target after rotation") - - except KeyboardInterrupt as e: - print('over') - alvik.stop() - sys.exit()