diff --git a/arduino_alvik.py b/arduino_alvik.py
index eb59935..c52a611 100644
--- a/arduino_alvik.py
+++ b/arduino_alvik.py
@@ -57,6 +57,7 @@ def begin(self) -> int:
         sleep_ms(100)
         self._reset_hw()
         sleep_ms(1000)
+        self.set_illuminator(True)
         return 0
 
     def _begin_update_thread(self):
@@ -276,6 +277,9 @@ def _parse_message(self) -> int:
         elif code == ord('q'):
             # imu position
             _, self.roll, self.pitch, self.yaw = self.packeter.unpacketC3F()
+        if code == ord('w'):
+            # wheels position
+            _, self.left_wheel._position, self.right_wheel._position = self.packeter.unpacketC2F()
         elif code == 0x7E:
             # firmware version
             _, *self.version = self.packeter.unpacketC3B()
@@ -390,6 +394,7 @@ def __init__(self, packeter: ucPack, label: int, wheel_diameter_mm: float = WHEE
         self._label = label
         self._wheel_diameter_mm = wheel_diameter_mm
         self._speed = None
+        self._position = None
 
     def reset(self, initial_position: float = 0.0):
         """
@@ -397,7 +402,8 @@ def reset(self, initial_position: float = 0.0):
         :param initial_position:
         :return:
         """
-        pass
+        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):
         """
@@ -408,7 +414,7 @@ def set_pid_gains(self, kp: float = MOTOR_KP_DEFAULT, ki: float = MOTOR_KI_DEFAU
         :return:
         """
 
-        self._packeter.packetC1B3F(ord('P'), self._label, kp, ki, kd)
+        self._packeter.packetC1B3F(ord('P'), self._label & 0xFF, kp, ki, kd)
         uart.write(self._packeter.msg[0:self._packeter.msg_size])
 
     def stop(self):
@@ -441,6 +447,13 @@ def get_speed(self) -> float:
         """
         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
diff --git a/examples/message_reader.py b/examples/message_reader.py
index ac5b49b..2d367f9 100644
--- a/examples/message_reader.py
+++ b/examples/message_reader.py
@@ -12,7 +12,9 @@
     try:
         print(f'VER: {alvik.version}')
         print(f'LSP: {alvik.left_wheel.get_speed()}')
-        print(f'RSP: {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}')
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()