diff --git a/arduino_alvik.py b/arduino_alvik.py
index f10dbe5..633ec49 100644
--- a/arduino_alvik.py
+++ b/arduino_alvik.py
@@ -1,3 +1,7 @@
+import math
+
+import gc
+
 from uart import uart
 import _thread
 from time import sleep_ms
@@ -5,21 +9,32 @@
 from ucPack import ucPack
 
 from pinout_definitions import *
+from robot_definitions import *
 from constants import *
 
 
 class ArduinoAlvik:
 
+    _update_thread_running = False
+    _update_thread_id = None
+
+    def __new__(cls):
+        if not hasattr(cls, 'instance'):
+            cls.instance = super(ArduinoAlvik, cls).__new__(cls)
+        return cls.instance
+
     def __init__(self):
         self.packeter = ucPack(200)
-        self._update_thread_running = False
-        self._update_thread_id = None
-        self.l_speed = None
-        self.r_speed = None
+        self.left_wheel = _ArduinoAlvikWheel(self.packeter, ord('L'))
+        self.right_wheel = _ArduinoAlvikWheel(self.packeter, ord('R'))
+        self.led_state = [None]
+        self.left_led = _ArduinoAlvikRgbLed(self.packeter, 'left', self.led_state,
+                                            rgb_mask=[0b00000100, 0b00001000, 0b00010000])
+        self.right_led = _ArduinoAlvikRgbLed(self.packeter, 'right', self.led_state,
+                                             rgb_mask=[0b00100000, 0b01000000, 0b10000000])
         self.battery_perc = None
         self.touch_bits = None
         self.behaviour = None
-        self.led_state = None
         self.red = None
         self.green = None
         self.blue = None
@@ -29,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
@@ -36,25 +60,108 @@ def __init__(self):
         self.right_tof = None
         self.top_tof = None
         self.bottom_tof = None
+        self.linear_velocity = None
+        self.angular_velocity = None
+        self.last_ack = ''
         self.version = [None, None, None]
 
-    def run(self):
+    def begin(self) -> int:
+        """
+        Begins all Alvik operations
+        :return:
+        """
+        if not CHECK_STM32.value():
+            print("\nTurn on your Arduino Alvik!\n")
+            return -1
+        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
+
+    def _begin_update_thread(self):
         """
         Runs robot background operations (e.g. threaded update)
         :return:
         """
-        self._update_thread_running = True
-        self._update_thread_id = _thread.start_new_thread(self._update, (1,))
 
-    def stop(self):
+        if not self.__class__._update_thread_running:
+            self.__class__._update_thread_running = True
+            self.__class__._update_thread_id = _thread.start_new_thread(self._update, (1,))
+
+    @classmethod
+    def _stop_update_thread(cls):
         """
         Stops the background operations
         :return:
         """
-        self._update_thread_running = False
+        cls._update_thread_running = False
+
+    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, 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):
+        """
+        Stops all Alvik operations
+        :return:
+        """
+        # stop engines
+        self.set_wheels_speed(0, 0)
+
+        # turn off UI leds
+        self._set_leds(0x00)
+
+        # stop the update thrad
+        self._stop_update_thread()
+
+        # delete instance
+        del self.__class__.instance
+        gc.collect()
 
     @staticmethod
-    def reset_hw():
+    def _reset_hw():
         """
         Resets the STM32
         :return:
@@ -65,48 +172,104 @@ def reset_hw():
         RESET_STM32.value(1)
         sleep_ms(100)
 
-    def get_speeds(self) -> (float, float):
-        return self.l_speed, self.r_speed
+    def get_wheels_speed(self) -> (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()
 
-    def set_speeds(self, left_speed: float, right_speed: float):
+    def set_wheels_speed(self, left_speed: float, right_speed: float, unit: str = 'rpm'):
         """
         Sets left/right motor speed
         :param left_speed:
         :param right_speed:
+        :param unit: the speed unit of measurement (default: 'rpm')
         :return:
         """
-        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:
-        """
+        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.packetC1B3F(ord('P'), ord(side), kp, ki, kd)
+        self.packeter.packetC2F(ord('J'), left_speed, right_speed)
         uart.write(self.packeter.msg[0:self.packeter.msg_size])
 
     def get_orientation(self) -> (float, float, float):
         """
         Returns the orientation of the IMU
-        :return:
+        :return: roll, pitch, yaw
         """
 
         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
-        :return:
+        :return: left_line, center_line, right_line
         """
 
         return self.left_line, self.center_line, self.right_line
 
+    def drive(self, linear_velocity: float, angular_velocity: float):
+        """
+        Drives the robot by linear and angular velocity
+        :param linear_velocity:
+        :param angular_velocity:
+        :return:
+        """
+        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):
+        """
+        Returns linear and angular velocity of the robot
+        :return: linear_velocity, angular_velocity
+        """
+        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
@@ -117,6 +280,13 @@ def set_servo_positions(self, a_position: int, b_position: int):
         self.packeter.packetC2B(ord('S'), a_position & 0xFF, b_position & 0xFF)
         uart.write(self.packeter.msg[0:self.packeter.msg_size])
 
+    def get_ack(self):
+        """
+        Resets and returns last acknowledgement
+        :return:
+        """
+        return self.last_ack
+
     # def send_ack(self):
     #     self.packeter.packetC1B(ord('X'), ACK_)
     #     uart.write(self.packeter.msg[0:self.packeter.msg_size])
@@ -128,37 +298,31 @@ def _set_leds(self, led_state: int):
         5->right_red 6->right_green 7->right_blue
         :return:
         """
-        self.led_state = led_state & 0xFF
-        self.packeter.packetC1B(ord('L'), self.led_state)
+        self.led_state[0] = led_state & 0xFF
+        self.packeter.packetC1B(ord('L'), self.led_state[0])
         uart.write(self.packeter.msg[0:self.packeter.msg_size])
 
     def set_builtin_led(self, value: bool):
-        if self.led_state is None:
+        """
+        Turns on/off the builtin led
+        :param value:
+        :return:
+        """
+        if self.led_state[0] is None:
             self._set_leds(0x00)
-        self.led_state = self.led_state | 0b00000001 if value else self.led_state & 0b11111110
-        self._set_leds(self.led_state)
+        self.led_state[0] = self.led_state[0] | 0b00000001 if value else self.led_state[0] & 0b11111110
+        self._set_leds(self.led_state[0])
 
     def set_illuminator(self, value: bool):
-        if self.led_state is None:
-            self._set_leds(0x00)
-        self.led_state = self.led_state | 0b00000010 if value else self.led_state & 0b11111101
-        self._set_leds(self.led_state)
-
-    def set_left_led_color(self, red: bool, green: bool, blue: bool):
-        if self.led_state is None:
-            self._set_leds(0x00)
-        self.led_state = self.led_state | 0b00000100 if red else self.led_state & 0b11111011
-        self.led_state = self.led_state | 0b00001000 if green else self.led_state & 0b11110111
-        self.led_state = self.led_state | 0b00010000 if blue else self.led_state & 0b11101111
-        self._set_leds(self.led_state)
-
-    def set_right_led_color(self, red: bool, green: bool, blue: bool):
-        if self.led_state is None:
+        """
+        Turns on/off the illuminator led
+        :param value:
+        :return:
+        """
+        if self.led_state[0] is None:
             self._set_leds(0x00)
-        self.led_state = self.led_state | 0b00100000 if red else self.led_state & 0b11011111
-        self.led_state = self.led_state | 0b01000000 if green else self.led_state & 0b10111111
-        self.led_state = self.led_state | 0b10000000 if blue else self.led_state & 0b01111111
-        self._set_leds(self.led_state)
+        self.led_state[0] = self.led_state[0] | 0b00000010 if value else self.led_state[0] & 0b11111101
+        self._set_leds(self.led_state[0])
 
     def _update(self, delay_=1):
         """
@@ -169,7 +333,7 @@ def _update(self, delay_=1):
         :return:
         """
         while True:
-            if not self._update_thread_running:
+            if not ArduinoAlvik._update_thread_running:
                 break
             if self._read_message():
                 self._parse_message()
@@ -196,7 +360,7 @@ def _parse_message(self) -> int:
         code = self.packeter.payload[0]
         if code == ord('j'):
             # joint speed
-            _, self.l_speed, self.r_speed = self.packeter.unpacketC2F()
+            _, self.left_wheel._speed, self.right_wheel._speed = self.packeter.unpacketC2F()
         elif code == ord('l'):
             # line sensor
             _, self.left_line, self.center_line, self.right_line = self.packeter.unpacketC3I()
@@ -205,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()
@@ -225,6 +389,18 @@ def _parse_message(self) -> int:
         elif code == ord('q'):
             # imu position
             _, self.roll, self.pitch, self.yaw = self.packeter.unpacketC3F()
+        elif code == ord('w'):
+            # wheels position
+            _, self.left_wheel._position, self.right_wheel._position = self.packeter.unpacketC2F()
+        elif code == ord('v'):
+            # robot velocity
+            _, self.linear_velocity, self.angular_velocity = self.packeter.unpacketC2F()
+        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()
@@ -234,36 +410,72 @@ def _parse_message(self) -> int:
         return 0
 
     def _get_touch(self) -> int:
+        """
+        Returns the touch sensor's state
+        :return: touch_bits
+        """
         return self.touch_bits
 
     def get_touch_any(self) -> bool:
+        """
+        Returns true if any button is pressed
+        :return:
+        """
         return bool(self.touch_bits & 0b00000001)
 
     def get_touch_ok(self) -> bool:
+        """
+        Returns true if ok button is pressed
+        :return:
+        """
         return bool(self.touch_bits & 0b00000010)
 
     def get_touch_cancel(self) -> bool:
+        """
+        Returns true if cancel button is pressed
+        :return:
+        """
         return bool(self.touch_bits & 0b00000100)
 
     def get_touch_center(self) -> bool:
+        """
+        Returns true if center button is pressed
+        :return:
+        """
         return bool(self.touch_bits & 0b00001000)
 
     def get_touch_up(self) -> bool:
+        """
+        Returns true if up button is pressed
+        :return:
+        """
         return bool(self.touch_bits & 0b00010000)
 
     def get_touch_left(self) -> bool:
+        """
+        Returns true if left button is pressed
+        :return:
+        """
         return bool(self.touch_bits & 0b00100000)
 
     def get_touch_down(self) -> bool:
+        """
+        Returns true if down button is pressed
+        :return:
+        """
         return bool(self.touch_bits & 0b01000000)
 
     def get_touch_right(self) -> bool:
+        """
+        Returns true if right button is pressed
+        :return:
+        """
         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 readout
-        :return:
+        Returns the color sensor's raw readout
+        :return: red, green, blue
         """
 
         return self.red, self.green, self.blue
@@ -272,13 +484,160 @@ def get_color(self) -> (int, int, int):
         #         int((self.blue/COLOR_FULL_SCALE)*255))
 
     def get_distance(self) -> (int, int, int, int, int, int):
+        """
+        Returns the distance readout of the TOF sensor
+        :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
 
     def get_version(self) -> str:
+        """
+        Returns the firmware version of the Alvik
+        :return:
+        """
         return f'{self.version[0]}.{self.version[1]}.{self.version[2]}'
 
     def print_status(self):
+        """
+        Prints the Alvik status
+        :return:
+        """
         for a in vars(self):
             if str(a).startswith('_'):
                 continue
             print(f'{str(a).upper()} = {getattr(self, str(a))}')
+
+
+class _ArduinoAlvikWheel:
+
+    def __init__(self, packeter: ucPack, label: int, wheel_diameter_mm: float = WHEEL_DIAMETER_MM):
+        self._packeter = packeter
+        self._label = label
+        self._wheel_diameter_mm = wheel_diameter_mm
+        self._speed = None
+        self._position = None
+
+    def reset(self, initial_position: float = 0.0):
+        """
+        Resets the wheel reference position
+        :param initial_position:
+        :return:
+        """
+        self._packeter.packetC2B1F(ord('W'), self._label & 0xFF, ord('Z'), initial_position)
+        uart.write(self._packeter.msg[0:self._packeter.msg_size])
+
+    def set_pid_gains(self, kp: float = MOTOR_KP_DEFAULT, ki: float = MOTOR_KI_DEFAULT, kd: float = MOTOR_KD_DEFAULT):
+        """
+        Set PID gains for Alvik wheels
+        :param kp: proportional gain
+        :param ki: integration gain
+        :param kd: derivative gain
+        :return:
+        """
+
+        self._packeter.packetC1B3F(ord('P'), self._label & 0xFF, kp, ki, kd)
+        uart.write(self._packeter.msg[0:self._packeter.msg_size])
+
+    def stop(self):
+        """
+        Stop Alvik wheel
+        :return:
+        """
+        self.set_speed(0)
+
+    def set_speed(self, velocity: float, unit: str = 'rpm'):
+        """
+        Sets the motor speed
+        :param velocity: the speed of the motor
+        :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)
+        uart.write(self._packeter.msg[0:self._packeter.msg_size])
+
+    def get_speed(self) -> float:
+        """
+        Returns the current RPM speed of the wheel
+        :return:
+        """
+        return self._speed
+
+    def get_position(self) -> float:
+        """
+        Returns the wheel position (angle with respect to the reference)
+        :return:
+        """
+        return self._position
+
+    def set_position(self, position: float, unit: str = 'deg'):
+        """
+        Sets left/right motor speed
+        :param position: the speed of the motor
+        :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)
+        uart.write(self._packeter.msg[0:self._packeter.msg_size])
+
+
+class _ArduinoAlvikRgbLed:
+    def __init__(self, packeter: ucPack, label: str, led_state: list[int | None], rgb_mask: list[int]):
+        self._packeter = packeter
+        self.label = label
+        self._rgb_mask = rgb_mask
+        self._led_state = led_state
+
+    def set_color(self, red: bool, green: bool, blue: bool):
+        """
+        Sets the LED's r,g,b state
+        :param red:
+        :param green:
+        :param blue:
+        :return:
+        """
+        led_status = self._led_state[0]
+        if led_status is None:
+            return
+        led_status = led_status | self._rgb_mask[0] if red else led_status & (0b11111111 - self._rgb_mask[0])
+        led_status = led_status | self._rgb_mask[1] if green else led_status & (0b11111111 - self._rgb_mask[1])
+        led_status = led_status | self._rgb_mask[2] if blue else led_status & (0b11111111 - self._rgb_mask[2])
+        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/examples/led_setting.py b/examples/leds_setting.py
similarity index 57%
rename from examples/led_setting.py
rename to examples/leds_setting.py
index cc2c005..cceb6c4 100644
--- a/examples/led_setting.py
+++ b/examples/leds_setting.py
@@ -3,17 +3,10 @@
 import sys
 
 alvik = ArduinoAlvik()
-
-alvik.run()
-sleep_ms(100)
-alvik.reset_hw()
+alvik.begin()
 
 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)
@@ -22,18 +15,23 @@
         sleep_ms(1000)
         alvik.set_illuminator(0)
         sleep_ms(1000)
-        alvik.set_left_led_color(0,0,1)
+        alvik.left_led.set_color(0, 0, 1)
+        sleep_ms(1000)
+        alvik.left_led.set_color(0, 1, 0)
+        sleep_ms(1000)
+        alvik.left_led.set_color(1, 0, 0)
         sleep_ms(1000)
-        alvik.set_right_led_color(0,0,1)
+        alvik.left_led.set_color(1, 1, 1)
         sleep_ms(1000)
-        alvik.set_left_led_color(0,1,0)
+        alvik.right_led.set_color(0, 0, 1)
         sleep_ms(1000)
-        alvik.set_right_led_color(0,1,0)
+        alvik.right_led.set_color(0, 1, 0)
         sleep_ms(1000)
-        alvik.set_left_led_color(1,0,0)
+        alvik.right_led.set_color(1, 0, 0)
         sleep_ms(1000)
-        alvik.set_right_led_color(1,0,0)
+        alvik.right_led.set_color(1, 1, 1)
         sleep_ms(1000)
     except KeyboardInterrupt as e:
         print('over')
+        alvik.stop()
         sys.exit()
diff --git a/examples/message_reader.py b/examples/message_reader.py
index 89086f8..9c0a582 100644
--- a/examples/message_reader.py
+++ b/examples/message_reader.py
@@ -3,26 +3,27 @@
 import sys
 
 alvik = ArduinoAlvik()
+if alvik.begin() < 0:
+    sys.exit()
 
-alvik.run()
-sleep_ms(100)
-alvik.reset_hw()
 speed = 0
 
 while True:
     try:
         print(f'VER: {alvik.version}')
-        print(f'LSP: {alvik.l_speed}')
-        print(f'RSP: {alvik.r_speed}')
+        print(f'LSP: {alvik.left_wheel.get_speed()}')
+        print(f'RSP: {alvik.right_wheel.get_speed()}')
+        print(f'LPOS: {alvik.left_wheel.get_position()}')
+        print(f'RPOS: {alvik.right_wheel.get_position()}')
         print(f'TOUCH: {alvik.touch_bits}')
         print(f'RGB: {alvik.red} {alvik.green} {alvik.blue}')
         print(f'LINE: {alvik.left_line} {alvik.center_line} {alvik.right_line}')
 
-        alvik.set_speeds(speed, speed)
+        alvik.set_wheels_speed(speed, speed)
         speed = (speed + 1) % 60
         sleep_ms(1000)
     except KeyboardInterrupt as e:
         print('over')
-        alvik.set_speeds(0, 0)
-        break
-sys.exit()
+        alvik.stop()
+        sys.exit()
+
diff --git a/examples/move_wheels.py b/examples/move_wheels.py
new file mode 100644
index 0000000..d6f6e7e
--- /dev/null
+++ b/examples/move_wheels.py
@@ -0,0 +1,36 @@
+from arduino_alvik import ArduinoAlvik
+from time import sleep_ms
+import sys
+
+alvik = ArduinoAlvik()
+alvik.begin()
+
+while True:
+    try:
+        alvik.left_wheel.set_speed(10)
+        sleep_ms(1000)
+        print(f'LSP: {alvik.left_wheel.get_speed()}')
+        print(f'RSP: {alvik.right_wheel.get_speed()}')
+
+        alvik.right_wheel.set_speed(10)
+        sleep_ms(1000)
+
+        print(f'LSP: {alvik.left_wheel.get_speed()}')
+        print(f'RSP: {alvik.right_wheel.get_speed()}')
+
+        alvik.left_wheel.set_speed(20)
+        sleep_ms(1000)
+
+        print(f'LSP: {alvik.left_wheel.get_speed()}')
+        print(f'RSP: {alvik.right_wheel.get_speed()}')
+
+        alvik.right_wheel.set_speed(20)
+        sleep_ms(1000)
+
+        print(f'LSP: {alvik.left_wheel.get_speed()}')
+        print(f'RSP: {alvik.right_wheel.get_speed()}')
+
+    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 aa4d1a4..27636b2 100644
--- a/examples/read_color_sensor.py
+++ b/examples/read_color_sensor.py
@@ -3,18 +3,15 @@
 import sys
 
 alvik = ArduinoAlvik()
-
-alvik.run()
-sleep_ms(1000)
-alvik.reset_hw()
+alvik.begin()
 speed = 0
 
 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:
         print('over')
-        break
-sys.exit()
+        alvik.stop()
+        sys.exit()
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/read_touch.py b/examples/read_touch.py
index 8ebf4a2..9f7e183 100644
--- a/examples/read_touch.py
+++ b/examples/read_touch.py
@@ -3,10 +3,7 @@
 import sys
 
 alvik = ArduinoAlvik()
-
-alvik.run()
-sleep_ms(1000)
-alvik.reset_hw()
+alvik.begin()
 speed = 0
 
 while True:
@@ -30,5 +27,5 @@
         sleep_ms(100)
     except KeyboardInterrupt as e:
         print('over')
-        break
-sys.exit()
+        alvik.stop()
+        sys.exit()
diff --git a/examples/set_pid.py b/examples/set_pid.py
index a3d21b1..77909f9 100644
--- a/examples/set_pid.py
+++ b/examples/set_pid.py
@@ -3,19 +3,16 @@
 import sys
 
 alvik = ArduinoAlvik()
-
-alvik.run()
-sleep_ms(1000)
-alvik.reset_hw()
+alvik.begin()
 speed = 0
 
 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')
-        break
-sys.exit()
+        alvik.stop()
+        sys.exit()
diff --git a/examples/wheels_servo.py b/examples/wheels_servo.py
new file mode 100644
index 0000000..b4220dd
--- /dev/null
+++ b/examples/wheels_servo.py
@@ -0,0 +1,36 @@
+from arduino_alvik import ArduinoAlvik
+from time import sleep
+import sys
+
+alvik = ArduinoAlvik()
+alvik.begin()
+
+alvik.left_wheel.reset()
+alvik.right_wheel.reset()
+
+while True:
+    try:
+        alvik.left_wheel.set_position(30)
+        sleep(2)
+        print(f'Left wheel degs: {alvik.left_wheel.get_position()}')
+        print(f'Right wheel degs: {alvik.right_wheel.get_position()}')
+
+        alvik.right_wheel.set_position(10)
+        sleep(2)
+        print(f'Left wheel degs: {alvik.left_wheel.get_position()}')
+        print(f'Right wheel degs: {alvik.right_wheel.get_position()}')
+
+        alvik.left_wheel.set_position(180)
+        sleep(2)
+        print(f'Left wheel degs: {alvik.left_wheel.get_position()}')
+        print(f'Right wheel degs: {alvik.right_wheel.get_position()}')
+
+        alvik.right_wheel.set_position(270)
+        sleep(2)
+        print(f'Left wheel degs: {alvik.left_wheel.get_position()}')
+        print(f'Right wheel degs: {alvik.right_wheel.get_position()}')
+
+    except KeyboardInterrupt as e:
+        print('over')
+        alvik.stop()
+        sys.exit()
diff --git a/examples/move_example.py b/examples/wheels_speed.py
similarity index 60%
rename from examples/move_example.py
rename to examples/wheels_speed.py
index ffc674b..18c9e46 100644
--- a/examples/move_example.py
+++ b/examples/wheels_speed.py
@@ -3,23 +3,19 @@
 import sys
 
 alvik = ArduinoAlvik()
-
-alvik.run()
-sleep_ms(100)
-alvik.reset_hw()
-
+alvik.begin()
 
 while True:
     try:
-        alvik.set_speeds(10, 10)
+        alvik.set_wheels_speed(10, 10)
         sleep_ms(1000)
 
-        alvik.set_speeds(30, 60)
+        alvik.set_wheels_speed(30, 60)
         sleep_ms(1000)
 
-        alvik.set_speeds(60, 30)
+        alvik.set_wheels_speed(60, 30)
         sleep_ms(1000)
     except KeyboardInterrupt as e:
         print('over')
-        alvik.set_speeds(0, 0)
+        alvik.stop()
         sys.exit()
diff --git a/install.bat b/install.bat
index e4b41d1..7871cc1 100644
--- a/install.bat
+++ b/install.bat
@@ -19,11 +19,13 @@ if /i "%1"=="-h" (
 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 :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 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
 
 echo Installing dependencies
diff --git a/install.sh b/install.sh
index 3e1821e..8129e2c 100644
--- a/install.sh
+++ b/install.sh
@@ -45,11 +45,13 @@ 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 :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 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
 
 echo "Installing dependencies"
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 b10a06a..a24e3d9 100644
--- a/pinout_definitions.py
+++ b/pinout_definitions.py
@@ -1,7 +1,17 @@
 from machine import Pin
 
 # NANO to STM32 PINS
-D2 = 5                              # ESP32 pin5 -> nano D2
-D3 = 6                              # ESP32 pin6 -> nano D3
-BOOT0_STM32 = Pin(D2, Pin.OUT)      # nano D2 -> STM32 Boot0
-RESET_STM32 = Pin(D3, Pin.OUT)      # nano D2 -> STM32 NRST
+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 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)
diff --git a/robot_definitions.py b/robot_definitions.py
new file mode 100644
index 0000000..610378e
--- /dev/null
+++ b/robot_definitions.py
@@ -0,0 +1,8 @@
+# Motor control and mechanical parameters
+MOTOR_KP_DEFAULT = 32.0
+MOTOR_KI_DEFAULT = 450.0
+MOTOR_KD_DEFAULT = 0.0
+MOTOR_MAX_RPM = 70
+
+# Wheels parameters
+WHEEL_DIAMETER_MM = 34