From aa78ebfc4058e7f175482f3d5616ffab6264ba35 Mon Sep 17 00:00:00 2001
From: Lucio Rossi <lucio.rossi75@gmail.com>
Date: Wed, 7 Feb 2024 18:18:59 +0100
Subject: [PATCH 01/26] feat: units conversion (to be tested)

---
 arduino_alvik.py | 96 +++++++++++++++++++-----------------------------
 conversions.py   | 80 ++++++++++++++++++++++++++++++++++++++++
 2 files changed, 118 insertions(+), 58 deletions(-)
 create mode 100644 conversions.py

diff --git a/arduino_alvik.py b/arduino_alvik.py
index 633ec49..9ace07f 100644
--- a/arduino_alvik.py
+++ b/arduino_alvik.py
@@ -8,6 +8,7 @@
 
 from ucPack import ucPack
 
+from conversions import *
 from pinout_definitions import *
 from robot_definitions import *
 from constants import *
@@ -116,26 +117,30 @@ def is_target_reached(self) -> bool:
             sleep_ms(200)
             return True
 
-    def rotate(self, angle: float, blocking: bool = True):
+    def rotate(self, angle: float, blocking: bool = True, unit: str = 'deg'):
         """
         Rotates the robot by given angle
         :param angle:
         :param blocking:
+        :param unit: the angle unit
         :return:
         """
+        angle = convert_angle(angle, unit, 'deg')
         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, blocking: bool = True):
+    def move(self, distance: float, blocking: bool = True, unit: str = 'cm'):
         """
         Moves the robot by given distance
         :param distance:
         :param blocking:
+        :param unit: the distance unit
         :return:
         """
+        distance = convert_distance(distance, unit, 'mm')
         sleep_ms(200)
         self.packeter.packetC1F(ord('G'), distance)
         uart.write(self.packeter.msg[0:self.packeter.msg_size])
@@ -187,14 +192,8 @@ def set_wheels_speed(self, left_speed: float, right_speed: float, unit: str = 'r
         :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)
+        self.packeter.packetC2F(ord('J'), convert_rotational_speed(left_speed, unit, 'rpm'),
+                                convert_rotational_speed(right_speed, unit, 'rpm'))
         uart.write(self.packeter.msg[0:self.packeter.msg_size])
 
     def get_orientation(self) -> (float, float, float):
@@ -234,13 +233,18 @@ 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):
+    def drive(self, linear_velocity: float, angular_velocity: float, linear_unit: str = 'cm/s',
+              angular_unit: str = 'deg/s'):
         """
         Drives the robot by linear and angular velocity
         :param linear_velocity:
         :param angular_velocity:
+        :param linear_unit:
+        :param angular_unit:
         :return:
         """
+        convert_speed(linear_velocity, linear_unit, 'mm/s')
+        convert_rotational_speed(angular_velocity, angular_unit, 'deg/s')
         self.packeter.packetC2F(ord('V'), linear_velocity, angular_velocity)
         uart.write(self.packeter.msg[0:self.packeter.msg_size])
 
@@ -263,12 +267,16 @@ def reset_pose(self, x: float, y: float, theta: float):
         uart.write(self.packeter.msg[0:self.packeter.msg_size])
         sleep_ms(1000)
 
-    def get_pose(self) -> (float, float, float):
+    def get_pose(self, distance_unit: str = 'cm', angle_unit: str = 'deg') -> (float, float, float):
         """
         Returns the current pose of the robot
+        :param distance_unit: unit of x and y outputs
+        :param angle_unit: unit of theta output
         :return: x, y, theta
         """
-        return self.x, self.y, self.theta
+        return (convert_distance(self.x, 'mm', distance_unit),
+                convert_distance(self.y, 'mm', distance_unit),
+                convert_angle(self.theta, 'deg', angle_unit))
 
     def set_servo_positions(self, a_position: int, b_position: int):
         """
@@ -483,12 +491,17 @@ def get_color_raw(self) -> (int, int, int):
         #         int((self.green/COLOR_FULL_SCALE)*255),
         #         int((self.blue/COLOR_FULL_SCALE)*255))
 
-    def get_distance(self) -> (int, int, int, int, int, int):
+    def get_distance(self, unit: str = 'cm') -> (int, int, int, int, int, int):
         """
         Returns the distance readout of the TOF sensor
+        :param unit: distance output unit
         :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
+        return (convert_distance(self.left_tof, 'mm', unit),
+                convert_distance(self.center_left_tof, 'mm', unit),
+                convert_distance(self.center_tof, 'mm', unit),
+                convert_distance(self.center_right_tof, 'mm', unit),
+                convert_distance(self.right_tof, 'mm', unit))
 
     def get_version(self) -> str:
         """
@@ -552,28 +565,25 @@ def set_speed(self, velocity: float, unit: str = 'rpm'):
         :param unit: the unit of measurement
         :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)
+        self._packeter.packetC2B1F(ord('W'), self._label & 0xFF, ord('V'),
+                                   convert_rotational_speed(velocity, unit, 'rpm'))
         uart.write(self._packeter.msg[0:self._packeter.msg_size])
 
-    def get_speed(self) -> float:
+    def get_speed(self, unit: str = 'rpm') -> float:
         """
         Returns the current RPM speed of the wheel
+        :param unit: the unit of the output speed
         :return:
         """
-        return self._speed
+        return convert_rotational_speed(self._speed, 'rpm', unit)
 
-    def get_position(self) -> float:
+    def get_position(self, unit: str = 'deg') -> float:
         """
         Returns the wheel position (angle with respect to the reference)
+        :param unit: the unit of the output position
         :return:
         """
-        return self._position
+        return convert_angle(self._position, 'deg', unit)
 
     def set_position(self, position: float, unit: str = 'deg'):
         """
@@ -582,13 +592,8 @@ 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)
+        self._packeter.packetC2B1F(ord('W'), self._label & 0xFF, ord('P'),
+                                   convert_angle(position, unit, 'deg'))
         uart.write(self._packeter.msg[0:self._packeter.msg_size])
 
 
@@ -616,28 +621,3 @@ 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:
-    """
-    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
diff --git a/conversions.py b/conversions.py
new file mode 100644
index 0000000..5925cc8
--- /dev/null
+++ b/conversions.py
@@ -0,0 +1,80 @@
+# MEASUREMENT UNITS CONVERSION #
+
+from math import pi
+
+
+def conversion_method(func):
+    def wrapper(*args, **kwargs):
+        try:
+            return func(*args, **kwargs)
+        except KeyError:
+            raise ConversionError(f'Cannot {func.__name__} from {args[1]} to {args[2]}')
+    return wrapper
+
+
+@conversion_method
+def convert_rotational_speed(value: float, from_unit: str, to_unit: str):
+    """
+    Converts a rotational speed value from one unit to another
+    :param value:
+    :param from_unit: unit of input value
+    :param to_unit: unit of output value
+    :return:
+    """
+    speeds = {'rpm': 1.0, 'deg/s': 1/6, 'rad/s': 60/2*pi}
+    return value * speeds[from_unit.lower()] / speeds[to_unit.lower()]
+
+
+@conversion_method
+def convert_angle(value: float, from_unit: str, to_unit: str):
+    """
+    Converts an angle value from one unit to another
+    :param value:
+    :param from_unit: unit of input value
+    :param to_unit: unit of output value
+    :return:
+    """
+    angles = {'deg': 1.0, 'rad': 180/pi, 'rev': 360, 'revolution': 360, '%': 3.6, 'perc': 3.6}
+    return value * angles[from_unit.lower()] / angles[to_unit.lower()]
+
+
+@conversion_method
+def convert_distance(value: float, from_unit: str, to_unit: str):
+    """
+    Converts a distance value from one unit to another
+    :param value:
+    :param from_unit: unit of input value
+    :param to_unit: unit of output value
+    :return:
+    """
+    distances = {'cm': 1.0, 'mm': 0.1, 'm': 100, 'inch': 2.54, 'in': 2.54}
+    return value * distances[from_unit.lower()] / distances[to_unit.lower()]
+
+
+@conversion_method
+def convert_speed(value: float, from_unit: str, to_unit: str):
+    """
+    Converts a distance value from one unit to another
+    :param value:
+    :param from_unit: unit of input value
+    :param to_unit: unit of output value
+    :return:
+    """
+    distances = {'cm/s': 1.0, 'mm/s': 0.1, 'm/s': 100, 'inch/s': 2.54, 'in/s': 2.54}
+    return value * distances[from_unit.lower()] / distances[to_unit.lower()]
+
+
+class ConversionError(Exception):
+    pass
+
+
+if __name__ == '__main__':
+    print(convert_rotational_speed(1, 'rpm', 'deg/s'))
+    print(convert_rotational_speed(1, 'deg/s', 'rpm'))
+    print(convert_angle(360, 'deg', 'rad'))
+    print(convert_angle(pi, 'rad', '%'))
+    print(convert_angle(0.25, 'rev', 'deg'))
+    print(convert_angle(0.25, 'REV', 'perc'))
+    print(convert_distance(10, 'mm', 'cm'))
+    print(convert_distance(1, 'in', 'mm'))
+    print(convert_rotational_speed(1, 'km/h', 'rpm'))

From 6990db324e900c1d454393430b47778ab52cf5ff Mon Sep 17 00:00:00 2001
From: Lucio Rossi <lucio.rossi75@gmail.com>
Date: Wed, 7 Feb 2024 18:34:34 +0100
Subject: [PATCH 02/26] feat: set_behaviuor with example

---
 arduino_alvik.py      |  9 +++++++++
 examples/behaviour.py | 14 ++++++++++++++
 2 files changed, 23 insertions(+)
 create mode 100644 examples/behaviour.py

diff --git a/arduino_alvik.py b/arduino_alvik.py
index 9ace07f..241738b 100644
--- a/arduino_alvik.py
+++ b/arduino_alvik.py
@@ -117,6 +117,15 @@ def is_target_reached(self) -> bool:
             sleep_ms(200)
             return True
 
+    def set_behaviour(self, behaviour: int):
+        """
+        Sets the behaviour of Alvik
+        :param behaviour: behaviour code
+        :return:
+        """
+        self.packeter.packetC1B(ord('B'), behaviour & 0xFF)
+        uart.write(self.packeter.msg[0:self.packeter.msg_size])
+
     def rotate(self, angle: float, blocking: bool = True, unit: str = 'deg'):
         """
         Rotates the robot by given angle
diff --git a/examples/behaviour.py b/examples/behaviour.py
new file mode 100644
index 0000000..9779e25
--- /dev/null
+++ b/examples/behaviour.py
@@ -0,0 +1,14 @@
+from arduino_alvik import ArduinoAlvik
+from time import sleep_ms
+import sys
+
+alvik = ArduinoAlvik()
+alvik.begin()
+
+while True:
+    try:
+        alvik.set_behaviour(behaviour=1)
+    except KeyboardInterrupt as e:
+        print('over')
+        alvik.stop()
+        sys.exit()

From b61552f858d2889bc97d292cb0a4ece166d82b4e Mon Sep 17 00:00:00 2001
From: Lucio Rossi <lucio.rossi75@gmail.com>
Date: Fri, 9 Feb 2024 15:49:22 +0100
Subject: [PATCH 03/26] fix: installer must include conversions.py module

---
 install.bat | 2 ++
 install.sh  | 2 ++
 2 files changed, 4 insertions(+)

diff --git a/install.bat b/install.bat
index 7871cc1..2e81c7c 100644
--- a/install.bat
+++ b/install.bat
@@ -18,12 +18,14 @@ if /i "%1"=="-h" (
 :install
 python -m mpremote %port_string% fs rm :arduino_alvik.py
 python -m mpremote %port_string% fs rm :constants.py
+python -m mpremote %port_string% fs rm :conversions.py
 python -m mpremote %port_string% fs rm :pinout_definitions.py
 python -m mpremote %port_string% fs rm :robot_definitions.py
 python -m mpremote %port_string% fs rm :uart.py
 
 python -m mpremote %port_string% fs cp arduino_alvik.py :arduino_alvik.py
 python -m mpremote %port_string% fs cp constants.py :constants.py
+python -m mpremote %port_string% fs cp conversions.py :conversions.py
 python -m mpremote %port_string% fs cp pinout_definitions.py :pinout_definitions.py
 python -m mpremote %port_string% fs cp robot_definitions.py :robot_definitions.py
 python -m mpremote %port_string% fs cp uart.py :uart.py
diff --git a/install.sh b/install.sh
index 8129e2c..c5950d4 100644
--- a/install.sh
+++ b/install.sh
@@ -44,12 +44,14 @@ fi
 
 $python_command -m mpremote $connect_string fs rm :arduino_alvik.py
 $python_command -m mpremote $connect_string fs rm :constants.py
+$python_command -m mpremote $connect_string fs rm :conversions.py
 $python_command -m mpremote $connect_string fs rm :pinout_definitions.py
 $python_command -m mpremote $connect_string fs rm :robot_definitions.py
 $python_command -m mpremote $connect_string fs rm :uart.py
 
 $python_command -m mpremote $connect_string fs cp arduino_alvik.py :arduino_alvik.py
 $python_command -m mpremote $connect_string fs cp constants.py :constants.py
+$python_command -m mpremote $connect_string fs cp conversions.py :conversions.py
 $python_command -m mpremote $connect_string fs cp pinout_definitions.py :pinout_definitions.py
 $python_command -m mpremote $connect_string fs cp robot_definitions.py :robot_definitions.py
 $python_command -m mpremote $connect_string fs cp uart.py :uart.py

From 71de028b4dd4423d2ee25fd1b26e54368c2d3456 Mon Sep 17 00:00:00 2001
From: Lucio Rossi <lucio.rossi75@gmail.com>
Date: Fri, 9 Feb 2024 16:49:13 +0100
Subject: [PATCH 04/26] feat: get/set wheels position at once

---
 arduino_alvik.py | 25 +++++++++++++++++++++++--
 1 file changed, 23 insertions(+), 2 deletions(-)

diff --git a/arduino_alvik.py b/arduino_alvik.py
index 241738b..987df9a 100644
--- a/arduino_alvik.py
+++ b/arduino_alvik.py
@@ -186,12 +186,13 @@ def _reset_hw():
         RESET_STM32.value(1)
         sleep_ms(100)
 
-    def get_wheels_speed(self) -> (float, float):
+    def get_wheels_speed(self, unit: str = 'rpm') -> (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()
+        return (convert_rotational_speed(self.left_wheel.get_speed(), 'rpm', unit),
+                convert_rotational_speed(self.right_wheel.get_speed(), 'rpm', unit))
 
     def set_wheels_speed(self, left_speed: float, right_speed: float, unit: str = 'rpm'):
         """
@@ -205,6 +206,26 @@ def set_wheels_speed(self, left_speed: float, right_speed: float, unit: str = 'r
                                 convert_rotational_speed(right_speed, unit, 'rpm'))
         uart.write(self.packeter.msg[0:self.packeter.msg_size])
 
+    def set_wheels_position(self, left_angle: float, right_angle: float, unit: str = 'deg'):
+        """
+        Sets left/right motor angle
+        :param left_angle:
+        :param right_angle:
+        :param unit: the speed unit of measurement (default: 'rpm')
+        :return:
+        """
+        self.packeter.packetC2F(ord('A'), convert_angle(left_angle, unit, 'deg'),
+                                convert_angle(right_angle, unit, 'deg'))
+        uart.write(self.packeter.msg[0:self.packeter.msg_size])
+
+    def get_wheels_position(self, unit: str = 'deg') -> (float, float):
+        """
+        Returns the angle of the wheels
+        :return: left_wheel_angle, right_wheel_angle
+        """
+        return (convert_angle(self.left_wheel.get_position(), 'deg', unit),
+                convert_angle(self.right_wheel.get_position(), 'deg', unit))
+
     def get_orientation(self) -> (float, float, float):
         """
         Returns the orientation of the IMU

From a935fced6d71616d630c684dcd5e20a0a02f9afd Mon Sep 17 00:00:00 2001
From: Lucio Rossi <lucio.rossi75@gmail.com>
Date: Fri, 9 Feb 2024 18:32:57 +0100
Subject: [PATCH 05/26] fix: convert rot speed from rad/s

---
 conversions.py | 2 +-
 1 file changed, 1 insertion(+), 1 deletion(-)

diff --git a/conversions.py b/conversions.py
index 5925cc8..56c0c69 100644
--- a/conversions.py
+++ b/conversions.py
@@ -21,7 +21,7 @@ def convert_rotational_speed(value: float, from_unit: str, to_unit: str):
     :param to_unit: unit of output value
     :return:
     """
-    speeds = {'rpm': 1.0, 'deg/s': 1/6, 'rad/s': 60/2*pi}
+    speeds = {'rpm': 1.0, 'deg/s': 1/6, 'rad/s': 60/(2*pi)}
     return value * speeds[from_unit.lower()] / speeds[to_unit.lower()]
 
 

From f94319959096f760517aaed8d8f97195c1f4b705 Mon Sep 17 00:00:00 2001
From: Lucio Rossi <lucio.rossi75@gmail.com>
Date: Mon, 12 Feb 2024 09:34:22 +0100
Subject: [PATCH 06/26] fix: ArduinoAlvik.drive not converting speeds

---
 arduino_alvik.py | 4 ++--
 1 file changed, 2 insertions(+), 2 deletions(-)

diff --git a/arduino_alvik.py b/arduino_alvik.py
index 987df9a..252a7a4 100644
--- a/arduino_alvik.py
+++ b/arduino_alvik.py
@@ -273,8 +273,8 @@ def drive(self, linear_velocity: float, angular_velocity: float, linear_unit: st
         :param angular_unit:
         :return:
         """
-        convert_speed(linear_velocity, linear_unit, 'mm/s')
-        convert_rotational_speed(angular_velocity, angular_unit, 'deg/s')
+        linear_velocity = convert_speed(linear_velocity, linear_unit, 'mm/s')
+        angular_velocity = convert_rotational_speed(angular_velocity, angular_unit, 'deg/s')
         self.packeter.packetC2F(ord('V'), linear_velocity, angular_velocity)
         uart.write(self.packeter.msg[0:self.packeter.msg_size])
 

From b0dc0ba25573f3e84b27179fbe274317a2c396d7 Mon Sep 17 00:00:00 2001
From: Lucio Rossi <lucio.rossi75@gmail.com>
Date: Mon, 12 Feb 2024 10:02:48 +0100
Subject: [PATCH 07/26] fix: Alvik _read_message exiting on byte = terminator
 mod: sleep 2s after receiving ack from robot

---
 arduino_alvik.py      | 6 +++---
 pinout_definitions.py | 6 +++---
 2 files changed, 6 insertions(+), 6 deletions(-)

diff --git a/arduino_alvik.py b/arduino_alvik.py
index 252a7a4..8968aa3 100644
--- a/arduino_alvik.py
+++ b/arduino_alvik.py
@@ -82,6 +82,7 @@ def begin(self) -> int:
         sleep_ms(1000)
         while self.last_ack != 0x00:
             sleep_ms(20)
+        sleep_ms(2000)
         self.set_illuminator(True)
         return 0
 
@@ -367,7 +368,7 @@ def _update(self, delay_=1):
         Updates the robot status reading/parsing messages from UART.
         This method is blocking and meant as a thread callback
         Use the method stop to terminate _update and exit the thread
-        :param delay_: while loop delay
+        :param delay_: while loop delay (ms)
         :return:
         """
         while True:
@@ -385,8 +386,7 @@ def _read_message(self) -> bool:
         while uart.any():
             b = uart.read(1)[0]
             self.packeter.buffer.push(b)
-            if b == self.packeter.end_index:
-                self.packeter.checkPayload()
+            if b == self.packeter.end_index and self.packeter.checkPayload():
                 return True
         return False
 
diff --git a/pinout_definitions.py b/pinout_definitions.py
index a24e3d9..5551eb1 100644
--- a/pinout_definitions.py
+++ b/pinout_definitions.py
@@ -11,7 +11,7 @@
 
 BOOT0_STM32 = Pin(D2, Pin.OUT)                  # nano D2 -> STM32 Boot0
 RESET_STM32 = Pin(D3, Pin.OUT)                  # nano D3 -> STM32 NRST
-NANO_CHK = Pin(D4, Pin.OUT)                     # nano D3 -> STM32 NRST
+NANO_CHK = Pin(D4, Pin.OUT)                     # nano D3 -> STM32 NANO_CHK
 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)
+ESP32_SDA = Pin(A4, Pin.OUT)                    # ESP32_SDA
+ESP32_SCL = Pin(A5, Pin.OUT)                    # ESP32_SCL

From 8f8ce11b951694c96f3ba18f34cc3a0b21885811 Mon Sep 17 00:00:00 2001
From: Lucio Rossi <lucio.rossi75@gmail.com>
Date: Mon, 12 Feb 2024 10:06:30 +0100
Subject: [PATCH 08/26] ALVIKDEV-49 use payloadTop

---
 arduino_alvik.py | 2 +-
 1 file changed, 1 insertion(+), 1 deletion(-)

diff --git a/arduino_alvik.py b/arduino_alvik.py
index 8968aa3..5c47cab 100644
--- a/arduino_alvik.py
+++ b/arduino_alvik.py
@@ -395,7 +395,7 @@ def _parse_message(self) -> int:
         Parse a received message
         :return: -1 if parse error 0 if ok
         """
-        code = self.packeter.payload[0]
+        code = self.packeter.payloadTop()
         if code == ord('j'):
             # joint speed
             _, self.left_wheel._speed, self.right_wheel._speed = self.packeter.unpacketC2F()

From fc0f9c055b5768f28b71718c01683f3ab784dfe8 Mon Sep 17 00:00:00 2001
From: Lucio Rossi <lucio.rossi75@gmail.com>
Date: Mon, 12 Feb 2024 10:45:13 +0100
Subject: [PATCH 09/26] feat: get_drive_speed conversion doc: conversions.py
 out types example: test_meas_units.py

---
 arduino_alvik.py            |  15 +++---
 conversions.py              |  20 ++-----
 examples/test_meas_units.py | 104 ++++++++++++++++++++++++++++++++++++
 3 files changed, 117 insertions(+), 22 deletions(-)
 create mode 100644 examples/test_meas_units.py

diff --git a/arduino_alvik.py b/arduino_alvik.py
index 5c47cab..e24513f 100644
--- a/arduino_alvik.py
+++ b/arduino_alvik.py
@@ -270,8 +270,8 @@ def drive(self, linear_velocity: float, angular_velocity: float, linear_unit: st
         Drives the robot by linear and angular velocity
         :param linear_velocity:
         :param angular_velocity:
-        :param linear_unit:
-        :param angular_unit:
+        :param linear_unit: output linear velocity unit of meas
+        :param angular_unit: output angular velocity unit of meas
         :return:
         """
         linear_velocity = convert_speed(linear_velocity, linear_unit, 'mm/s')
@@ -279,12 +279,15 @@ def drive(self, linear_velocity: float, angular_velocity: float, linear_unit: st
         self.packeter.packetC2F(ord('V'), linear_velocity, angular_velocity)
         uart.write(self.packeter.msg[0:self.packeter.msg_size])
 
-    def get_drive_speed(self) -> (float, float):
+    def get_drive_speed(self, linear_unit: str = 'mm/s', angular_unit: str = 'deg/s') -> (float, float):
         """
         Returns linear and angular velocity of the robot
+        :param linear_unit: output linear velocity unit of meas
+        :param angular_unit: output angular velocity unit of meas
         :return: linear_velocity, angular_velocity
         """
-        return self.linear_velocity, self.angular_velocity
+        return (convert_speed(self.linear_velocity, 'mm/s', linear_unit),
+                convert_rotational_speed(self.angular_velocity, 'deg/s', angular_unit))
 
     def reset_pose(self, x: float, y: float, theta: float):
         """
@@ -312,8 +315,8 @@ def get_pose(self, distance_unit: str = 'cm', angle_unit: str = 'deg') -> (float
     def set_servo_positions(self, a_position: int, b_position: int):
         """
         Sets A/B servomotor angle
-        :param a_position: position of A servomotor
-        :param b_position: position of B servomotor
+        :param a_position: position of A servomotor (0-180°)
+        :param b_position: position of B servomotor (0-180°)
         :return:
         """
         self.packeter.packetC2B(ord('S'), a_position & 0xFF, b_position & 0xFF)
diff --git a/conversions.py b/conversions.py
index 56c0c69..529574d 100644
--- a/conversions.py
+++ b/conversions.py
@@ -13,7 +13,7 @@ def wrapper(*args, **kwargs):
 
 
 @conversion_method
-def convert_rotational_speed(value: float, from_unit: str, to_unit: str):
+def convert_rotational_speed(value: float, from_unit: str, to_unit: str) -> float:
     """
     Converts a rotational speed value from one unit to another
     :param value:
@@ -26,7 +26,7 @@ def convert_rotational_speed(value: float, from_unit: str, to_unit: str):
 
 
 @conversion_method
-def convert_angle(value: float, from_unit: str, to_unit: str):
+def convert_angle(value: float, from_unit: str, to_unit: str) -> float:
     """
     Converts an angle value from one unit to another
     :param value:
@@ -39,7 +39,7 @@ def convert_angle(value: float, from_unit: str, to_unit: str):
 
 
 @conversion_method
-def convert_distance(value: float, from_unit: str, to_unit: str):
+def convert_distance(value: float, from_unit: str, to_unit: str) -> float:
     """
     Converts a distance value from one unit to another
     :param value:
@@ -52,7 +52,7 @@ def convert_distance(value: float, from_unit: str, to_unit: str):
 
 
 @conversion_method
-def convert_speed(value: float, from_unit: str, to_unit: str):
+def convert_speed(value: float, from_unit: str, to_unit: str) -> float:
     """
     Converts a distance value from one unit to another
     :param value:
@@ -66,15 +66,3 @@ def convert_speed(value: float, from_unit: str, to_unit: str):
 
 class ConversionError(Exception):
     pass
-
-
-if __name__ == '__main__':
-    print(convert_rotational_speed(1, 'rpm', 'deg/s'))
-    print(convert_rotational_speed(1, 'deg/s', 'rpm'))
-    print(convert_angle(360, 'deg', 'rad'))
-    print(convert_angle(pi, 'rad', '%'))
-    print(convert_angle(0.25, 'rev', 'deg'))
-    print(convert_angle(0.25, 'REV', 'perc'))
-    print(convert_distance(10, 'mm', 'cm'))
-    print(convert_distance(1, 'in', 'mm'))
-    print(convert_rotational_speed(1, 'km/h', 'rpm'))
diff --git a/examples/test_meas_units.py b/examples/test_meas_units.py
new file mode 100644
index 0000000..351efd0
--- /dev/null
+++ b/examples/test_meas_units.py
@@ -0,0 +1,104 @@
+from arduino_alvik import ArduinoAlvik
+from time import sleep_ms
+import sys
+
+alvik = ArduinoAlvik()
+alvik.begin()
+
+while True:
+    try:
+
+        # -- LINEAR MOVEMENTS --
+
+        print("Move fw 0.05 m")
+        alvik.move(0.05, unit='m')
+        sleep_ms(2000)
+
+        print("Move fw 10 cm")
+        alvik.move(5, unit='cm')
+        sleep_ms(2000)
+
+        print("Move bw 100 mm")
+        alvik.move(-100, unit='mm')
+        sleep_ms(2000)
+
+        print("Move fw 1 inch")
+        alvik.move(1, unit='in')
+        sleep_ms(2000)
+
+        # -- WHEEL ROTATIONS --
+        alvik.right_wheel.reset()
+        sleep_ms(2000)
+        curr_pos = alvik.right_wheel.get_position()
+        print(f'R wheel pos: {curr_pos}')
+        sleep_ms(2000)
+
+        print("Rotate right wheel 25% fw")
+        alvik.right_wheel.set_position(25, unit='%')
+        sleep_ms(2000)
+        curr_pos = alvik.right_wheel.get_position()
+        print(f'R wheel pos: {curr_pos}')
+
+        print("Rotate right wheel 90 deg bw")
+        alvik.right_wheel.set_position(-90, unit='deg')
+        sleep_ms(2000)
+        curr_pos = alvik.right_wheel.get_position()
+        print(f'R wheel pos: {curr_pos}')
+
+        print("Rotate right wheel pi rad fw")
+        alvik.right_wheel.set_position(3.14, unit='rad')
+        sleep_ms(2000)
+        curr_pos = alvik.right_wheel.get_position()
+        print(f'R wheel pos: {curr_pos}')
+
+        print("Rotate right wheel a quarter revolution bw")
+        alvik.right_wheel.set_position(-0.25, unit='rev')
+        sleep_ms(2000)
+        curr_pos = alvik.right_wheel.get_position()
+        print(f'R wheel pos: {curr_pos}')
+
+        # -- WHEELS SPEED --
+        print("Set speed 12 rpm (1 rev in 5 sec)")
+        alvik.set_wheels_speed(12, 12, 'rpm')
+        sleep_ms(1000)
+        print(f"Current speed is {alvik.get_wheels_speed()} rpm")
+
+        print("Set speed -pi rad/s (1 back rev in 2 sec)")
+        alvik.set_wheels_speed(-3.1415, -3.1415, 'rad/s')
+        sleep_ms(1000)
+        print(f"Current speed is {alvik.get_wheels_speed()} rpm")
+
+        print("Set speed 180 deg/s (1 back rev in 2 sec)")
+        alvik.set_wheels_speed(180, 180, 'deg/s')
+        sleep_ms(1000)
+        print(f"Current speed is {alvik.get_wheels_speed()} rpm")
+
+
+        # -- DRIVE --
+        print("Driving at 10 mm/s (expecting approx 5.6 rpm)")
+        alvik.drive(10, 0, linear_unit='mm/s')
+        sleep_ms(2000)
+        print(f"Current speed is {alvik.get_wheels_speed()} rpm")
+
+        print("Driving at 2 cm/s (expecting approx 11.2 rpm)")
+        alvik.drive(2, 0, linear_unit='cm/s')
+        sleep_ms(2000)
+        print(f"Current speed is {alvik.get_wheels_speed()} rpm")
+
+        print("Driving at 1 in/s (expecting approx 14 rpm)")
+        alvik.drive(1, 0, linear_unit='in/s')
+        sleep_ms(2000)
+        print(f"Current speed is {alvik.get_wheels_speed()} rpm")
+
+        print("Driving at 5 mm/s (expecting approx 5.6 rpm) pi/8 rad/s (22.5 deg/s)")
+        alvik.drive(5, 3.1415/8, linear_unit='mm/s', angular_unit='rad/s')
+        sleep_ms(2000)
+        print(f"Current speed is {alvik.get_drive_speed()} rpm")
+
+        alvik.stop()
+        sys.exit()
+
+    except KeyboardInterrupt as e:
+        print('over')
+        alvik.stop()
+        sys.exit()

From 131f5a5d0e0e455b7bc6f991a7f993e44798e809 Mon Sep 17 00:00:00 2001
From: Lucio Rossi <lucio.rossi75@gmail.com>
Date: Mon, 12 Feb 2024 18:17:30 +0100
Subject: [PATCH 10/26] feat: wheel.reset and alvik.reset_pose w unit
 conversion feat: convert_rotational_speed added %rpm and rev/s

---
 arduino_alvik.py     | 13 ++++++++++---
 conversions.py       |  4 +++-
 robot_definitions.py |  5 +++--
 3 files changed, 16 insertions(+), 6 deletions(-)

diff --git a/arduino_alvik.py b/arduino_alvik.py
index e24513f..e570dad 100644
--- a/arduino_alvik.py
+++ b/arduino_alvik.py
@@ -142,7 +142,7 @@ def rotate(self, angle: float, blocking: bool = True, unit: str = 'deg'):
         if blocking:
             self._wait_for_target()
 
-    def move(self, distance: float, blocking: bool = True, unit: str = 'cm'):
+    def move(self, distance: float, unit: str = 'cm', blocking: bool = True):
         """
         Moves the robot by given distance
         :param distance:
@@ -289,14 +289,19 @@ def get_drive_speed(self, linear_unit: str = 'mm/s', angular_unit: str = 'deg/s'
         return (convert_speed(self.linear_velocity, 'mm/s', linear_unit),
                 convert_rotational_speed(self.angular_velocity, 'deg/s', angular_unit))
 
-    def reset_pose(self, x: float, y: float, theta: float):
+    def reset_pose(self, x: float, y: float, theta: float, distance_unit: str = 'cm', angle_unit: str = 'deg'):
         """
         Resets the robot pose
         :param x: x coordinate of the robot
         :param y: y coordinate of the robot
         :param theta: angle of the robot
+        :param distance_unit: angle of the robot
+        :param angle_unit: angle of the robot
         :return:
         """
+        x = convert_distance(x, distance_unit, 'mm')
+        y = convert_distance(y, distance_unit, 'mm')
+        theta = convert_angle(theta, angle_unit, 'deg')
         self.packeter.packetC3F(ord('Z'), x, y, theta)
         uart.write(self.packeter.msg[0:self.packeter.msg_size])
         sleep_ms(1000)
@@ -563,12 +568,14 @@ def __init__(self, packeter: ucPack, label: int, wheel_diameter_mm: float = WHEE
         self._speed = None
         self._position = None
 
-    def reset(self, initial_position: float = 0.0):
+    def reset(self, initial_position: float = 0.0, unit: str = 'deg'):
         """
         Resets the wheel reference position
         :param initial_position:
+        :param unit: reference position unit (defaults to deg)
         :return:
         """
+        initial_position = convert_angle(initial_position, unit, 'deg')
         self._packeter.packetC2B1F(ord('W'), self._label & 0xFF, ord('Z'), initial_position)
         uart.write(self._packeter.msg[0:self._packeter.msg_size])
 
diff --git a/conversions.py b/conversions.py
index 529574d..d413623 100644
--- a/conversions.py
+++ b/conversions.py
@@ -2,6 +2,8 @@
 
 from math import pi
 
+from robot_definitions import MOTOR_MAX_RPM
+
 
 def conversion_method(func):
     def wrapper(*args, **kwargs):
@@ -21,7 +23,7 @@ def convert_rotational_speed(value: float, from_unit: str, to_unit: str) -> floa
     :param to_unit: unit of output value
     :return:
     """
-    speeds = {'rpm': 1.0, 'deg/s': 1/6, 'rad/s': 60/(2*pi)}
+    speeds = {'rpm': 1.0, 'deg/s': 1/6, 'rad/s': 60/(2*pi), '%': MOTOR_MAX_RPM/100, 'rev/s': 60}
     return value * speeds[from_unit.lower()] / speeds[to_unit.lower()]
 
 
diff --git a/robot_definitions.py b/robot_definitions.py
index 610378e..4f7eea0 100644
--- a/robot_definitions.py
+++ b/robot_definitions.py
@@ -2,7 +2,8 @@
 MOTOR_KP_DEFAULT = 32.0
 MOTOR_KI_DEFAULT = 450.0
 MOTOR_KD_DEFAULT = 0.0
-MOTOR_MAX_RPM = 70
+MOTOR_MAX_RPM = 70.0
+WHEEL_TRACK_MM = 89.0
 
 # Wheels parameters
-WHEEL_DIAMETER_MM = 34
+WHEEL_DIAMETER_MM = 34.0

From 81e488f96d4d849976ee0933f4fbb6f8a59cc72e Mon Sep 17 00:00:00 2001
From: Lucio Rossi <lucio.rossi75@gmail.com>
Date: Mon, 12 Feb 2024 18:30:46 +0100
Subject: [PATCH 11/26] feat: conversion example with updated tests

---
 examples/test_meas_units.py | 14 ++++++++++++--
 1 file changed, 12 insertions(+), 2 deletions(-)

diff --git a/examples/test_meas_units.py b/examples/test_meas_units.py
index 351efd0..9f1dd84 100644
--- a/examples/test_meas_units.py
+++ b/examples/test_meas_units.py
@@ -26,6 +26,12 @@
         alvik.move(1, unit='in')
         sleep_ms(2000)
 
+        print(f"Current position: {alvik.get_pose()}")
+        alvik.reset_pose(0, 0, theta=3.1415, angle_unit='rad')
+        sleep_ms(2000)
+
+        print(f"Current position: {alvik.get_pose()}")
+
         # -- WHEEL ROTATIONS --
         alvik.right_wheel.reset()
         sleep_ms(2000)
@@ -73,7 +79,6 @@
         sleep_ms(1000)
         print(f"Current speed is {alvik.get_wheels_speed()} rpm")
 
-
         # -- DRIVE --
         print("Driving at 10 mm/s (expecting approx 5.6 rpm)")
         alvik.drive(10, 0, linear_unit='mm/s')
@@ -93,7 +98,12 @@
         print("Driving at 5 mm/s (expecting approx 5.6 rpm) pi/8 rad/s (22.5 deg/s)")
         alvik.drive(5, 3.1415/8, linear_unit='mm/s', angular_unit='rad/s')
         sleep_ms(2000)
-        print(f"Current speed is {alvik.get_drive_speed()} rpm")
+        print(f"Current speed is {alvik.get_drive_speed()} (mm/s) (rpm)")
+
+        print("Driving at 5 mm/s (expecting approx 5.6 rpm) 1/8 rev/s (45 deg/s)")
+        alvik.drive(5, 1/8, linear_unit='mm/s', angular_unit='rev/s')
+        sleep_ms(2000)
+        print(f"Current speed is {alvik.get_drive_speed()} (mm/s) (rpm)")
 
         alvik.stop()
         sys.exit()

From e68635b7183590637955a176eac6e93c903e66be Mon Sep 17 00:00:00 2001
From: Lucio Rossi <lucio.rossi75@gmail.com>
Date: Wed, 14 Feb 2024 09:46:48 +0100
Subject: [PATCH 12/26] fix: too many out params on get_distance interface

---
 arduino_alvik.py | 2 +-
 1 file changed, 1 insertion(+), 1 deletion(-)

diff --git a/arduino_alvik.py b/arduino_alvik.py
index e570dad..6ee3cbc 100644
--- a/arduino_alvik.py
+++ b/arduino_alvik.py
@@ -529,7 +529,7 @@ def get_color_raw(self) -> (int, int, int):
         #         int((self.green/COLOR_FULL_SCALE)*255),
         #         int((self.blue/COLOR_FULL_SCALE)*255))
 
-    def get_distance(self, unit: str = 'cm') -> (int, int, int, int, int, int):
+    def get_distance(self, unit: str = 'cm') -> (int, int, int, int, int):
         """
         Returns the distance readout of the TOF sensor
         :param unit: distance output unit

From a42160433d3cf23d50d071fafcfaed8ac1ba4394 Mon Sep 17 00:00:00 2001
From: Lucio Rossi <lucio.rossi75@gmail.com>
Date: Wed, 14 Feb 2024 11:21:45 +0100
Subject: [PATCH 13/26] feat: set_behaviuor inside begin

---
 arduino_alvik.py | 1 +
 1 file changed, 1 insertion(+)

diff --git a/arduino_alvik.py b/arduino_alvik.py
index 6ee3cbc..5b6da76 100644
--- a/arduino_alvik.py
+++ b/arduino_alvik.py
@@ -84,6 +84,7 @@ def begin(self) -> int:
             sleep_ms(20)
         sleep_ms(2000)
         self.set_illuminator(True)
+        self.set_behaviour(1)
         return 0
 
     def _begin_update_thread(self):

From 6fddffe65409a7be717be673cde6a2e8011f843f Mon Sep 17 00:00:00 2001
From: Lucio Rossi <lucio.rossi75@gmail.com>
Date: Wed, 14 Feb 2024 11:30:23 +0100
Subject: [PATCH 14/26] mod: roatate: blocking as last param

---
 arduino_alvik.py | 2 +-
 1 file changed, 1 insertion(+), 1 deletion(-)

diff --git a/arduino_alvik.py b/arduino_alvik.py
index 5b6da76..3eafdf5 100644
--- a/arduino_alvik.py
+++ b/arduino_alvik.py
@@ -128,7 +128,7 @@ def set_behaviour(self, behaviour: int):
         self.packeter.packetC1B(ord('B'), behaviour & 0xFF)
         uart.write(self.packeter.msg[0:self.packeter.msg_size])
 
-    def rotate(self, angle: float, blocking: bool = True, unit: str = 'deg'):
+    def rotate(self, angle: float, unit: str = 'deg', blocking: bool = True):
         """
         Rotates the robot by given angle
         :param angle:

From cb2a602a1c473bdb58451e24e2eddf4c5af60fb7 Mon Sep 17 00:00:00 2001
From: Lucio Rossi <lucio.rossi75@gmail.com>
Date: Wed, 14 Feb 2024 12:48:19 +0100
Subject: [PATCH 15/26] mod: conversions in % of MOTOR_MAX_RPM and
 ROBOT_MAX_DEG_S

---
 arduino_alvik.py            | 44 +++++++++++++++++++++++++++----------
 conversions.py              |  4 +---
 examples/test_meas_units.py | 10 +++++++++
 robot_definitions.py        |  3 +++
 4 files changed, 46 insertions(+), 15 deletions(-)

diff --git a/arduino_alvik.py b/arduino_alvik.py
index 3eafdf5..0dccb45 100644
--- a/arduino_alvik.py
+++ b/arduino_alvik.py
@@ -1,5 +1,3 @@
-import math
-
 import gc
 
 from uart import uart
@@ -193,8 +191,7 @@ def get_wheels_speed(self, unit: str = 'rpm') -> (float, float):
         Returns the speed of the wheels
         :return: left_wheel_speed, right_wheel_speed
         """
-        return (convert_rotational_speed(self.left_wheel.get_speed(), 'rpm', unit),
-                convert_rotational_speed(self.right_wheel.get_speed(), 'rpm', unit))
+        return self.left_wheel.get_speed(unit), self.right_wheel.get_speed(unit)
 
     def set_wheels_speed(self, left_speed: float, right_speed: float, unit: str = 'rpm'):
         """
@@ -204,8 +201,15 @@ def set_wheels_speed(self, left_speed: float, right_speed: float, unit: str = 'r
         :param unit: the speed unit of measurement (default: 'rpm')
         :return:
         """
-        self.packeter.packetC2F(ord('J'), convert_rotational_speed(left_speed, unit, 'rpm'),
-                                convert_rotational_speed(right_speed, unit, 'rpm'))
+
+        if unit == '%':
+            left_speed = (left_speed/100)*MOTOR_MAX_RPM
+            right_speed = (right_speed/100)*MOTOR_MAX_RPM
+        else:
+            left_speed = convert_rotational_speed(left_speed, unit, 'rpm')
+            right_speed = convert_rotational_speed(right_speed, unit, 'rpm')
+
+        self.packeter.packetC2F(ord('J'), left_speed, right_speed)
         uart.write(self.packeter.msg[0:self.packeter.msg_size])
 
     def set_wheels_position(self, left_angle: float, right_angle: float, unit: str = 'deg'):
@@ -276,7 +280,10 @@ def drive(self, linear_velocity: float, angular_velocity: float, linear_unit: st
         :return:
         """
         linear_velocity = convert_speed(linear_velocity, linear_unit, 'mm/s')
-        angular_velocity = convert_rotational_speed(angular_velocity, angular_unit, 'deg/s')
+        if angular_unit == '%':
+            angular_velocity = (angular_velocity/100)*ROBOT_MAX_DEG_S
+        else:
+            angular_velocity = convert_rotational_speed(angular_velocity, angular_unit, 'deg/s')
         self.packeter.packetC2F(ord('V'), linear_velocity, angular_velocity)
         uart.write(self.packeter.msg[0:self.packeter.msg_size])
 
@@ -287,8 +294,12 @@ def get_drive_speed(self, linear_unit: str = 'mm/s', angular_unit: str = 'deg/s'
         :param angular_unit: output angular velocity unit of meas
         :return: linear_velocity, angular_velocity
         """
-        return (convert_speed(self.linear_velocity, 'mm/s', linear_unit),
-                convert_rotational_speed(self.angular_velocity, 'deg/s', angular_unit))
+        if angular_unit == '%':
+            angular_velocity = (self.angular_velocity/ROBOT_MAX_DEG_S)*100
+        else:
+            angular_velocity = convert_rotational_speed(self.angular_velocity, 'deg/s', angular_unit)
+
+        return convert_speed(self.linear_velocity, 'mm/s', linear_unit), angular_velocity
 
     def reset_pose(self, x: float, y: float, theta: float, distance_unit: str = 'cm', angle_unit: str = 'deg'):
         """
@@ -606,8 +617,13 @@ def set_speed(self, velocity: float, unit: str = 'rpm'):
         :param unit: the unit of measurement
         :return:
         """
-        self._packeter.packetC2B1F(ord('W'), self._label & 0xFF, ord('V'),
-                                   convert_rotational_speed(velocity, unit, 'rpm'))
+
+        if unit == '%':
+            velocity = (velocity/100)*MOTOR_MAX_RPM
+        else:
+            velocity = convert_rotational_speed(velocity, unit, '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, unit: str = 'rpm') -> float:
@@ -616,7 +632,11 @@ def get_speed(self, unit: str = 'rpm') -> float:
         :param unit: the unit of the output speed
         :return:
         """
-        return convert_rotational_speed(self._speed, 'rpm', unit)
+        if unit == '%':
+            speed = (self._speed/MOTOR_MAX_RPM)*100
+        else:
+            speed = convert_rotational_speed(self._speed, 'rpm', unit)
+        return speed
 
     def get_position(self, unit: str = 'deg') -> float:
         """
diff --git a/conversions.py b/conversions.py
index d413623..d2cc96c 100644
--- a/conversions.py
+++ b/conversions.py
@@ -2,8 +2,6 @@
 
 from math import pi
 
-from robot_definitions import MOTOR_MAX_RPM
-
 
 def conversion_method(func):
     def wrapper(*args, **kwargs):
@@ -23,7 +21,7 @@ def convert_rotational_speed(value: float, from_unit: str, to_unit: str) -> floa
     :param to_unit: unit of output value
     :return:
     """
-    speeds = {'rpm': 1.0, 'deg/s': 1/6, 'rad/s': 60/(2*pi), '%': MOTOR_MAX_RPM/100, 'rev/s': 60}
+    speeds = {'rpm': 1.0, 'deg/s': 1/6, 'rad/s': 60/(2*pi), 'rev/s': 60}
     return value * speeds[from_unit.lower()] / speeds[to_unit.lower()]
 
 
diff --git a/examples/test_meas_units.py b/examples/test_meas_units.py
index 9f1dd84..468073d 100644
--- a/examples/test_meas_units.py
+++ b/examples/test_meas_units.py
@@ -64,6 +64,11 @@
         print(f'R wheel pos: {curr_pos}')
 
         # -- WHEELS SPEED --
+        print("Set speed 50% max_rpm (35.0 rpm)")
+        alvik.set_wheels_speed(50, 50, '%')
+        sleep_ms(1000)
+        print(f"Current speed is {alvik.get_wheels_speed()} rpm")
+
         print("Set speed 12 rpm (1 rev in 5 sec)")
         alvik.set_wheels_speed(12, 12, 'rpm')
         sleep_ms(1000)
@@ -80,6 +85,11 @@
         print(f"Current speed is {alvik.get_wheels_speed()} rpm")
 
         # -- DRIVE --
+        print("Driving at 10 mm/s (expecting approx 5.6 rpm 64 deg/s)")
+        alvik.drive(10, 20, linear_unit='mm/s', angular_unit='%')
+        sleep_ms(2000)
+        print(f"Current speed is {alvik.get_drive_speed()} (mm/s, deg/s))")
+
         print("Driving at 10 mm/s (expecting approx 5.6 rpm)")
         alvik.drive(10, 0, linear_unit='mm/s')
         sleep_ms(2000)
diff --git a/robot_definitions.py b/robot_definitions.py
index 4f7eea0..142adfa 100644
--- a/robot_definitions.py
+++ b/robot_definitions.py
@@ -7,3 +7,6 @@
 
 # Wheels parameters
 WHEEL_DIAMETER_MM = 34.0
+
+# Robot params
+ROBOT_MAX_DEG_S = 6*(2*MOTOR_MAX_RPM*WHEEL_DIAMETER_MM)/WHEEL_TRACK_MM

From e7ab95b716e63e3c5b5524bd8c74977ae73115ad Mon Sep 17 00:00:00 2001
From: Lucio Rossi <lucio.rossi75@gmail.com>
Date: Wed, 14 Feb 2024 14:54:08 +0100
Subject: [PATCH 16/26] feat: get_distance_top get_distance_bottom fix: get
 methods using conversions must always return floats

---
 arduino_alvik.py | 18 +++++++++++++++++-
 1 file changed, 17 insertions(+), 1 deletion(-)

diff --git a/arduino_alvik.py b/arduino_alvik.py
index 0dccb45..3e05c9a 100644
--- a/arduino_alvik.py
+++ b/arduino_alvik.py
@@ -541,7 +541,7 @@ def get_color_raw(self) -> (int, int, int):
         #         int((self.green/COLOR_FULL_SCALE)*255),
         #         int((self.blue/COLOR_FULL_SCALE)*255))
 
-    def get_distance(self, unit: str = 'cm') -> (int, int, int, int, int):
+    def get_distance(self, unit: str = 'cm') -> (float, float, float, float, float):
         """
         Returns the distance readout of the TOF sensor
         :param unit: distance output unit
@@ -553,6 +553,22 @@ def get_distance(self, unit: str = 'cm') -> (int, int, int, int, int):
                 convert_distance(self.center_right_tof, 'mm', unit),
                 convert_distance(self.right_tof, 'mm', unit))
 
+    def get_distance_top(self, unit: str = 'cm') -> float:
+        """
+        Returns the obstacle top distance readout
+        :param unit:
+        :return:
+        """
+        return convert_distance(self.top_tof, 'mm', unit)
+
+    def get_distance_bottom(self, unit: str = 'cm') -> float:
+        """
+        Returns the obstacle bottom distance readout
+        :param unit:
+        :return:
+        """
+        return convert_distance(self.bottom_tof, 'mm', unit)
+
     def get_version(self) -> str:
         """
         Returns the firmware version of the Alvik

From b2ca2d05146ee0570551f925900699e1ed178980 Mon Sep 17 00:00:00 2001
From: Lucio Rossi <lucio.rossi75@gmail.com>
Date: Wed, 14 Feb 2024 15:03:38 +0100
Subject: [PATCH 17/26] ex: read_tof.py

---
 examples/read_tof.py | 18 ++++++++++++++++++
 1 file changed, 18 insertions(+)
 create mode 100644 examples/read_tof.py

diff --git a/examples/read_tof.py b/examples/read_tof.py
new file mode 100644
index 0000000..990bf63
--- /dev/null
+++ b/examples/read_tof.py
@@ -0,0 +1,18 @@
+from arduino_alvik import ArduinoAlvik
+from time import sleep_ms
+import sys
+
+alvik = ArduinoAlvik()
+alvik.begin()
+
+while True:
+    try:
+        L, CL, C, CR, R = alvik.get_distance()
+        T = alvik.get_distance_top()
+        B = alvik.get_distance_bottom()
+        print(f'T: {T} | B: {B} | L: {L} | CL: {CL} | C: {C} | CR: {CR} | R: {R}')
+        sleep_ms(100)
+    except KeyboardInterrupt as e:
+        print('over')
+        alvik.stop()
+        sys.exit()

From d960ee22d60a4884c4168be9f10eaf0e116d6d00 Mon Sep 17 00:00:00 2001
From: Lucio Rossi <lucio.rossi75@gmail.com>
Date: Wed, 14 Feb 2024 15:44:42 +0100
Subject: [PATCH 18/26] fix: reading tof top and bottom inverted

---
 arduino_alvik.py | 2 +-
 1 file changed, 1 insertion(+), 1 deletion(-)

diff --git a/arduino_alvik.py b/arduino_alvik.py
index 3e05c9a..7e8d7b2 100644
--- a/arduino_alvik.py
+++ b/arduino_alvik.py
@@ -443,7 +443,7 @@ def _parse_message(self) -> int:
         elif code == ord('f'):
             # tof matrix
             (_, self.left_tof, self.center_left_tof, self.center_tof,
-             self.center_right_tof, self.right_tof, self.bottom_tof, self.top_tof) = self.packeter.unpacketC7I()
+             self.center_right_tof, self.right_tof, self.top_tof, self.bottom_tof) = self.packeter.unpacketC7I()
         elif code == ord('q'):
             # imu position
             _, self.roll, self.pitch, self.yaw = self.packeter.unpacketC3F()

From 20a492a38d75bf36174f1146531eab00696accab Mon Sep 17 00:00:00 2001
From: Lucio Rossi <lucio.rossi75@gmail.com>
Date: Wed, 14 Feb 2024 18:16:02 +0100
Subject: [PATCH 19/26] feat: b/w color calibration with defaults

---
 arduino_alvik.py | 68 ++++++++++++++++++++++++++++++++++++++++++++++++
 constants.py     |  2 ++
 2 files changed, 70 insertions(+)

diff --git a/arduino_alvik.py b/arduino_alvik.py
index 7e8d7b2..693f2c6 100644
--- a/arduino_alvik.py
+++ b/arduino_alvik.py
@@ -37,6 +37,8 @@ def __init__(self):
         self.red = None
         self.green = None
         self.blue = None
+        self._white_cal = None
+        self._black_cal = None
         self.left_line = None
         self.center_line = None
         self.right_line = None
@@ -83,6 +85,7 @@ def begin(self) -> int:
         sleep_ms(2000)
         self.set_illuminator(True)
         self.set_behaviour(1)
+        self._set_color_reference()
         return 0
 
     def _begin_update_thread(self):
@@ -530,6 +533,71 @@ def get_touch_right(self) -> bool:
         """
         return bool(self.touch_bits & 0b10000000)
 
+    def _set_color_reference(self):
+        try:
+            from color_calibration import BLACK_CAL as _B
+        except ImportError:
+            _B = BLACK_CAL
+        try:
+            from color_calibration import WHITE_CAL as _W
+        except ImportError:
+            _W = WHITE_CAL
+
+        self._black_cal = _B
+        self._white_cal = _W
+
+    def color_calibration(self, background: str = 'white') -> None:
+        """
+        Calibrates the color sensor
+        :param background: str white or black
+        :return:
+        """
+        if background not in ['black', 'white']:
+            return
+
+        red_avg = green_avg = blue_avg = 0
+
+        for _ in range(0, 100):
+            red, green, blue = self.get_color_raw()
+            red_avg += red
+            green_avg += green
+            blue_avg += blue
+            sleep_ms(10)
+
+        red_avg = int(red_avg/100)
+        green_avg = int(green_avg/100)
+        blue_avg = int(blue_avg/100)
+
+        if background == 'white':
+            self._white_cal = [red_avg, green_avg, blue_avg]
+        elif background == 'black':
+            self._black_cal = [red_avg, green_avg, blue_avg]
+
+        file_path = './color_calibration.py'
+
+        try:
+            with open(file_path, 'r') as file:
+                content = file.read().split('\n')
+                lines = [l + '\n' for l in content if l]
+        except OSError:
+            open(file_path, 'a').close()
+            lines = []
+
+        found_param_line = False
+
+        for i, line in enumerate(lines):
+            if line.startswith(background.upper()):
+                lines[i] = f'{background.upper()}_CAL = [{red_avg}, {green_avg}, {blue_avg}]\n'
+                found_param_line = True
+                break
+
+        if not found_param_line:
+            lines.extend([f'{background.upper()}_CAL = [{red_avg}, {green_avg}, {blue_avg}]\n'])
+
+        with open(file_path, 'w') as file:
+            for line in lines:
+                file.write(line)
+
     def get_color_raw(self) -> (int, int, int):
         """
         Returns the color sensor's raw readout
diff --git a/constants.py b/constants.py
index f505f47..1076df3 100644
--- a/constants.py
+++ b/constants.py
@@ -1,3 +1,5 @@
 
 # COLOR SENSOR
 COLOR_FULL_SCALE = 4097
+WHITE_CAL = [444, 342, 345]
+BLACK_CAL = [153, 135, 123]

From a484ef5f646b280d1bd12e17015b27dcf4cd7aa2 Mon Sep 17 00:00:00 2001
From: Lucio Rossi <lucio.rossi75@gmail.com>
Date: Fri, 16 Feb 2024 09:58:20 +0100
Subject: [PATCH 20/26] feat: color rgb, hsv and normalization

---
 arduino_alvik.py              | 90 +++++++++++++++++++++++++++++++++--
 examples/read_color_sensor.py |  6 +--
 2 files changed, 90 insertions(+), 6 deletions(-)

diff --git a/arduino_alvik.py b/arduino_alvik.py
index 693f2c6..dc91c22 100644
--- a/arduino_alvik.py
+++ b/arduino_alvik.py
@@ -533,6 +533,22 @@ def get_touch_right(self) -> bool:
         """
         return bool(self.touch_bits & 0b10000000)
 
+    @staticmethod
+    def _limit(value: float, lower: float, upper: float) -> float:
+        """
+        Utility function to limit a value between a lower and upper limit
+        :param value:
+        :param lower:
+        :param upper:
+        :return:
+        """
+        assert lower < upper
+        if value > upper:
+            value = upper
+        if value < lower:
+            value = lower
+        return value
+
     def _set_color_reference(self):
         try:
             from color_calibration import BLACK_CAL as _B
@@ -605,9 +621,77 @@ def get_color_raw(self) -> (int, int, int):
         """
 
         return self.red, self.green, self.blue
-        # return (int((self.red/COLOR_FULL_SCALE)*255),
-        #         int((self.green/COLOR_FULL_SCALE)*255),
-        #         int((self.blue/COLOR_FULL_SCALE)*255))
+
+    def _normalize_color(self, r: float, g: float, b: float) -> (float, float, float):
+        """
+        Color normalization
+        :param r:
+        :param g:
+        :param b:
+        :return:
+        """
+        r = self._limit(r, self._black_cal[0], self._white_cal[0])
+        g = self._limit(g, self._black_cal[1], self._white_cal[1])
+        b = self._limit(b, self._black_cal[2], self._white_cal[2])
+
+        r = (r - self._black_cal[0])/(self._white_cal[0] - self._black_cal[0])
+        g = (g - self._black_cal[1])/(self._white_cal[1] - self._black_cal[1])
+        b = (b - self._black_cal[2])/(self._white_cal[2] - self._black_cal[2])
+
+        return r, g, b
+
+    @staticmethod
+    def rgb2hsv(r: float, g: float, b: float) -> (float, float, float):
+        """
+        Converts normalized rgb to hsv
+        :param r:
+        :param g:
+        :param b:
+        :return:
+        """
+        min_ = min(r, g, b)
+        max_ = max(r, g, b)
+
+        v = max_
+        delta = max_ - min_
+
+        if delta < 0.00001:
+            h = 0
+            s = 0
+            return h, s, v
+
+        if max_ > 0:
+            s = delta / max_
+        else:
+            s = 0
+            h = None
+            return h, s, v
+
+        if r >= max_:
+            h = (g - b) / delta  # color is between yellow and magenta
+        elif g >= max_:
+            h = 2.0 + (b - r) / delta
+        else:
+            h = 4.0 + (r - g) / delta
+
+        h *= 60.0
+        if h < 0:
+            h += 360.0
+
+        return h, s, v
+
+    def get_color(self, color_format: str = 'rgb') -> (float, float, float):
+        """
+        Returns the normalized color readout of the color sensor
+        :param color_format: rgb or hsv only
+        :return:
+        """
+        assert color_format in ['rgb', 'hsv']
+
+        if color_format == 'rgb':
+            return self._normalize_color(*self.get_color_raw())
+        elif color_format == 'hsv':
+            return self.rgb2hsv(*self._normalize_color(*self.get_color_raw()))
 
     def get_distance(self, unit: str = 'cm') -> (float, float, float, float, float):
         """
diff --git a/examples/read_color_sensor.py b/examples/read_color_sensor.py
index 27636b2..9f65d98 100644
--- a/examples/read_color_sensor.py
+++ b/examples/read_color_sensor.py
@@ -4,12 +4,12 @@
 
 alvik = ArduinoAlvik()
 alvik.begin()
-speed = 0
 
 while True:
     try:
-        r, g, b = alvik.get_color_raw()
-        print(f'RED: {r}, Green: {g}, Blue: {b}')
+        r, g, b = alvik.get_color()
+        h, s, v = alvik.get_color('hsv')
+        print(f'RED: {r}, Green: {g}, Blue: {b}, HUE: {h}, SAT: {s}, VAL: {v}')
         sleep_ms(100)
     except KeyboardInterrupt as e:
         print('over')

From a1bae8a22ff8e74d3e64c72e6fdebd07e4c7c63a Mon Sep 17 00:00:00 2001
From: Lucio Rossi <lucio.rossi75@gmail.com>
Date: Fri, 16 Feb 2024 11:41:12 +0100
Subject: [PATCH 21/26] feat: hsv color labeling

---
 arduino_alvik.py              | 40 +++++++++++++++++++++++++++++++++++
 examples/read_color_sensor.py |  1 +
 2 files changed, 41 insertions(+)

diff --git a/arduino_alvik.py b/arduino_alvik.py
index dc91c22..b546226 100644
--- a/arduino_alvik.py
+++ b/arduino_alvik.py
@@ -692,6 +692,46 @@ def get_color(self, color_format: str = 'rgb') -> (float, float, float):
             return self._normalize_color(*self.get_color_raw())
         elif color_format == 'hsv':
             return self.rgb2hsv(*self._normalize_color(*self.get_color_raw()))
+    @staticmethod
+    def get_color_label(h, s, v) -> str:
+        label = 'UNDEFINED'
+
+        if s < 0.1:
+            if v < 0.05:
+                label = 'BLACK'
+            elif v < 0.15:
+                label = 'GREY'
+            elif v < 0.8:
+                label = 'LIGHT GREY'
+            else:
+                label = 'WHITE'
+        else:
+            if v > 0.1:
+                if 20 <= h < 90:
+                    label = 'YELLOW'
+                elif 90 <= h < 140:
+                    label = 'LIGHT GREEN'
+                elif 140 <= h < 170:
+                    label = 'GREEN'
+                elif 170 <= h < 210:
+                    label = 'LIGHT BLUE'
+                elif 210 <= h < 260:
+                    label = 'BLUE'
+                elif 260 <= h < 280:
+                    label = 'VIOLET'
+                elif 260 <= h < 280:
+                    label = 'VIOLET'
+                else:                   # h<20 or h>=280 is more problematic
+                    if v < 0.5 and s < 0.45:
+                        label = 'BROWN'
+                    else:
+                        if v > 0.77:
+                            label = 'ORANGE'
+                        else:
+                            label = 'RED'
+            else:
+                label = 'BLACK'
+        return label
 
     def get_distance(self, unit: str = 'cm') -> (float, float, float, float, float):
         """
diff --git a/examples/read_color_sensor.py b/examples/read_color_sensor.py
index 9f65d98..994dbca 100644
--- a/examples/read_color_sensor.py
+++ b/examples/read_color_sensor.py
@@ -10,6 +10,7 @@
         r, g, b = alvik.get_color()
         h, s, v = alvik.get_color('hsv')
         print(f'RED: {r}, Green: {g}, Blue: {b}, HUE: {h}, SAT: {s}, VAL: {v}')
+        print(f'COLOR LABEL: {alvik.get_color_label(h, s, v)}')
         sleep_ms(100)
     except KeyboardInterrupt as e:
         print('over')

From 1aeaac3bff536e36d96b77bcc7af5064e69d51a8 Mon Sep 17 00:00:00 2001
From: Lucio Rossi <lucio.rossi75@gmail.com>
Date: Fri, 16 Feb 2024 12:57:23 +0100
Subject: [PATCH 22/26] feat: brake method mod: increased uart baud

---
 arduino_alvik.py | 7 +++++++
 uart.py          | 2 +-
 2 files changed, 8 insertions(+), 1 deletion(-)

diff --git a/arduino_alvik.py b/arduino_alvik.py
index b546226..cf4344d 100644
--- a/arduino_alvik.py
+++ b/arduino_alvik.py
@@ -290,6 +290,13 @@ def drive(self, linear_velocity: float, angular_velocity: float, linear_unit: st
         self.packeter.packetC2F(ord('V'), linear_velocity, angular_velocity)
         uart.write(self.packeter.msg[0:self.packeter.msg_size])
 
+    def brake(self):
+        """
+        Brakes the robot
+        :return:
+        """
+        self.drive(0, 0)
+
     def get_drive_speed(self, linear_unit: str = 'mm/s', angular_unit: str = 'deg/s') -> (float, float):
         """
         Returns linear and angular velocity of the robot
diff --git a/uart.py b/uart.py
index 951ea42..8d73724 100644
--- a/uart.py
+++ b/uart.py
@@ -4,7 +4,7 @@
 _UART_ID = 1
 _TX_PIN = 43
 _RX_PIN = 44
-_BAUDRATE = 115200
+_BAUDRATE = 460800
 _BITS = 8
 _PARITY = None
 _STOP = 1

From 73229c871d51a30c457f3e3c70b15fef547a208b Mon Sep 17 00:00:00 2001
From: Lucio Rossi <lucio.rossi75@gmail.com>
Date: Fri, 16 Feb 2024 14:36:26 +0100
Subject: [PATCH 23/26] fix: get_drive_speed linear_speed must default to cm/s

---
 arduino_alvik.py | 2 +-
 1 file changed, 1 insertion(+), 1 deletion(-)

diff --git a/arduino_alvik.py b/arduino_alvik.py
index cf4344d..9161470 100644
--- a/arduino_alvik.py
+++ b/arduino_alvik.py
@@ -297,7 +297,7 @@ def brake(self):
         """
         self.drive(0, 0)
 
-    def get_drive_speed(self, linear_unit: str = 'mm/s', angular_unit: str = 'deg/s') -> (float, float):
+    def get_drive_speed(self, linear_unit: str = 'cm/s', angular_unit: str = 'deg/s') -> (float, float):
         """
         Returns linear and angular velocity of the robot
         :param linear_unit: output linear velocity unit of meas

From 9c59fbd8774917e88505a01bd33ace4da878232b Mon Sep 17 00:00:00 2001
From: Lucio Rossi <lucio.rossi75@gmail.com>
Date: Fri, 16 Feb 2024 16:21:05 +0100
Subject: [PATCH 24/26] examples: review

---
 examples/behaviour.py      | 14 --------------
 examples/message_reader.py |  2 --
 examples/pose_example.py   | 32 ++++++++++++--------------------
 examples/read_imu.py       |  1 -
 examples/read_touch.py     |  1 -
 examples/set_pid.py        | 18 ------------------
 examples/wheels_speed.py   |  3 +++
 7 files changed, 15 insertions(+), 56 deletions(-)
 delete mode 100644 examples/behaviour.py
 delete mode 100644 examples/set_pid.py

diff --git a/examples/behaviour.py b/examples/behaviour.py
deleted file mode 100644
index 9779e25..0000000
--- a/examples/behaviour.py
+++ /dev/null
@@ -1,14 +0,0 @@
-from arduino_alvik import ArduinoAlvik
-from time import sleep_ms
-import sys
-
-alvik = ArduinoAlvik()
-alvik.begin()
-
-while True:
-    try:
-        alvik.set_behaviour(behaviour=1)
-    except KeyboardInterrupt as e:
-        print('over')
-        alvik.stop()
-        sys.exit()
diff --git a/examples/message_reader.py b/examples/message_reader.py
index 9c0a582..57b3dd0 100644
--- a/examples/message_reader.py
+++ b/examples/message_reader.py
@@ -6,8 +6,6 @@
 if alvik.begin() < 0:
     sys.exit()
 
-speed = 0
-
 while True:
     try:
         print(f'VER: {alvik.version}')
diff --git a/examples/pose_example.py b/examples/pose_example.py
index 3f4f40e..3004d4d 100644
--- a/examples/pose_example.py
+++ b/examples/pose_example.py
@@ -8,56 +8,48 @@
 while True:
     try:
 
-        alvik.move(100.0)
+        alvik.move(100.0, 'mm')
         print("on target after move")
 
-        alvik.move(50.0)
+        alvik.move(50.0, 'mm')
         print("on target after move")
 
-        alvik.rotate(90.0)
+        alvik.rotate(90.0, 'deg')
         print("on target after rotation")
 
-        alvik.rotate(-45.00)
+        alvik.rotate(-45.00, 'deg')
         print("on target after rotation")
 
         x, y, theta = alvik.get_pose()
-        print(f'Current pose is x={x}, y={y} ,theta={theta}')
+        print(f'Current pose is x(cm)={x}, y(cm)={y}, theta(deg)={theta}')
 
         alvik.reset_pose(0, 0, 0)
 
         x, y, theta = alvik.get_pose()
-        print(f'Updated pose is x={x}, y={y} ,theta={theta}')
+        print(f'Updated pose is x(cm)={x}, y(cm)={y}, theta(deg)={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}")
+        alvik.move(50.0, 'mm', blocking=False)
         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}")
+        alvik.rotate(45.0, 'deg', blocking=False)
         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}")
+        alvik.move(100.0, 'mm', blocking=False)
         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}")
+        alvik.rotate(-90.00, 'deg', blocking=False)
         print("on target after rotation")
 
         x, y, theta = alvik.get_pose()
-        print(f'Current pose is x={x}, y={y} ,theta={theta}')
+        print(f'Current pose is x(cm)={x}, y(cm)={y}, theta(deg)={theta}')
 
         alvik.reset_pose(0, 0, 0)
 
         x, y, theta = alvik.get_pose()
-        print(f'Updated pose is x={x}, y={y} ,theta={theta}')
+        print(f'Updated pose is x={x}, y={y}, theta(deg)={theta}')
         sleep_ms(500)
 
         alvik.stop()
diff --git a/examples/read_imu.py b/examples/read_imu.py
index 9e82380..3c6589b 100644
--- a/examples/read_imu.py
+++ b/examples/read_imu.py
@@ -4,7 +4,6 @@
 
 alvik = ArduinoAlvik()
 alvik.begin()
-speed = 0
 
 while True:
     try:
diff --git a/examples/read_touch.py b/examples/read_touch.py
index 9f7e183..21c5ebb 100644
--- a/examples/read_touch.py
+++ b/examples/read_touch.py
@@ -4,7 +4,6 @@
 
 alvik = ArduinoAlvik()
 alvik.begin()
-speed = 0
 
 while True:
     try:
diff --git a/examples/set_pid.py b/examples/set_pid.py
deleted file mode 100644
index 77909f9..0000000
--- a/examples/set_pid.py
+++ /dev/null
@@ -1,18 +0,0 @@
-from arduino_alvik import ArduinoAlvik
-from time import sleep_ms
-import sys
-
-alvik = ArduinoAlvik()
-alvik.begin()
-speed = 0
-
-while True:
-    try:
-        alvik.left_wheel.set_pid_gains(10.0, 1.3, 4.2)
-        sleep_ms(100)
-        alvik.right_wheel.set_pid_gains(4.0, 13, 1.9)
-        sleep_ms(100)
-    except KeyboardInterrupt as e:
-        print('over')
-        alvik.stop()
-        sys.exit()
diff --git a/examples/wheels_speed.py b/examples/wheels_speed.py
index 18c9e46..1919f27 100644
--- a/examples/wheels_speed.py
+++ b/examples/wheels_speed.py
@@ -8,12 +8,15 @@
 while True:
     try:
         alvik.set_wheels_speed(10, 10)
+        print(f'Wheels speed: {alvik.get_wheels_speed()}')
         sleep_ms(1000)
 
         alvik.set_wheels_speed(30, 60)
+        print(f'Wheels speed: {alvik.get_wheels_speed()}')
         sleep_ms(1000)
 
         alvik.set_wheels_speed(60, 30)
+        print(f'Wheels speed: {alvik.get_wheels_speed()}')
         sleep_ms(1000)
     except KeyboardInterrupt as e:
         print('over')

From af7bc06074229a10cefd23c0074cf091421a1073 Mon Sep 17 00:00:00 2001
From: Lucio Rossi <lucio.rossi75@gmail.com>
Date: Fri, 16 Feb 2024 16:38:46 +0100
Subject: [PATCH 25/26] examples: hand_follower.py

---
 examples/hand_follower.py | 20 ++++++++++++++++++++
 1 file changed, 20 insertions(+)
 create mode 100644 examples/hand_follower.py

diff --git a/examples/hand_follower.py b/examples/hand_follower.py
new file mode 100644
index 0000000..5a031f3
--- /dev/null
+++ b/examples/hand_follower.py
@@ -0,0 +1,20 @@
+from arduino_alvik import ArduinoAlvik
+from time import sleep_ms
+import sys
+
+alvik = ArduinoAlvik()
+alvik.begin()
+
+reference = 10.0
+
+while True:
+    try:
+        L, CL, C, CR, R = alvik.get_distance()
+        print(f'C: {C}')
+        error = C - reference
+        alvik.set_wheels_speed(error*10, error*10)
+        sleep_ms(100)
+    except KeyboardInterrupt as e:
+        print('over')
+        alvik.stop()
+        sys.exit()
\ No newline at end of file

From 45d984763398a72232fbc412de12e7eb4fd4c475 Mon Sep 17 00:00:00 2001
From: Lucio Rossi <lucio.rossi75@gmail.com>
Date: Fri, 16 Feb 2024 16:40:01 +0100
Subject: [PATCH 26/26] release 0.2.0

---
 package.json | 2 +-
 1 file changed, 1 insertion(+), 1 deletion(-)

diff --git a/package.json b/package.json
index 556f12b..72a44a7 100644
--- a/package.json
+++ b/package.json
@@ -3,5 +3,5 @@
     ],
     "deps": [
     ],
-    "version": "0.1.0"
+    "version": "0.2.0"
 }
\ No newline at end of file