Merge pull request #6 from arduino/api_update

gbr1 · web-flow · commit a271be57d2b0 · 2024-02-02T17:53:30.000+01:00
Api update
diff --git 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
@@ -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
diff --git a/examples/pose_example.py 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
@@ -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
@@ -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
@@ -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
@@ -3,5 +3,5 @@
     ],
     "deps": [
     ],
-    "version": "0.0.7"
+    "version": "0.1.0"
 }
diff --git 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)

Original file line number	Diff line number	Diff line change
`@@ -3,5 +3,5 @@`
`3`	`3`	`],`
`4`	`4`	`"deps": [`
`5`	`5`	`],`
`6`		`- "version": "0.0.7"`
	`6`	`+ "version": "0.1.0"`
`7`	`7`	`}`