From 0e2c7745fde3c27bfa337ceb1f1b137f6cfefa17 Mon Sep 17 00:00:00 2001 From: Lucio Rossi Date: Mon, 22 Jan 2024 09:48:40 +0100 Subject: [PATCH 1/9] 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 2/9] 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 3/9] 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 4/9] 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 5/9] 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 6/9] 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 7/9] 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 8/9] 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 9/9] 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