diff --git a/arduino_alvik.py b/arduino_alvik.py index acd0d90..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 @@ -42,6 +44,15 @@ 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 + self.gx = None + self.gy = None + self.gz = None self.left_tof = None self.center_left_tof = None self.center_tof = None @@ -65,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 @@ -87,23 +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): """ @@ -119,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(): """ @@ -156,19 +197,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 @@ -177,6 +205,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_gyros(self) -> (float, float, float): + """ + Returns the 3-axial angular acceleration of the IMU + :return: gx, gy, gz + """ + return self.gx, self.gy, self.gz + + 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 @@ -202,6 +251,25 @@ def get_drive_speed(self) -> (float, float): """ return self.linear_velocity, self.angular_velocity + def reset_pose(self, x: float, y: float, theta: float): + """ + Resets 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]) + sleep_ms(1000) + + 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 @@ -301,7 +369,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() @@ -330,6 +398,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() @@ -401,9 +472,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/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() diff --git a/examples/pose_example.py b/examples/pose_example.py new file mode 100644 index 0000000..3f4f40e --- /dev/null +++ b/examples/pose_example.py @@ -0,0 +1,69 @@ +from arduino_alvik import ArduinoAlvik +from time import sleep_ms +import sys + +alvik = ArduinoAlvik() +alvik.begin() + +while True: + try: + + alvik.move(100.0) + print("on target after move") + + alvik.move(50.0) + print("on target after move") + + alvik.rotate(90.0) + print("on target after rotation") + + 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.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() + 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) + + alvik.stop() + sys.exit() + + 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 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: diff --git a/examples/read_imu.py b/examples/read_imu.py new file mode 100644 index 0000000..9e82380 --- /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_gyros() + 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() 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') 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 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)