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

Author: eigen-value

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():
         """

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()