From fde782d78a148fffd4d4dbed8e5a6d056c33d96b Mon Sep 17 00:00:00 2001 From: Lucio Rossi Date: Wed, 31 Jan 2024 12:37:53 +0100 Subject: [PATCH 01/12] 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 02/12] 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 03/12] 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 04/12] 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 05/12] 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 06/12] 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 07/12] 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 08/12] 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 09/12] 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 10/12] 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 11/12] 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 12/12] 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()