diff --git a/examples/firmware_01/firmware_01.ino b/examples/firmware_01/firmware_01.ino
index 0f8c0ae..18c8865 100644
--- a/examples/firmware_01/firmware_01.ino
+++ b/examples/firmware_01/firmware_01.ino
@@ -90,7 +90,7 @@ void loop(){
         }
         
         break;
-      
+
       case 'S':
         packeter.unpacketC2B(code,servo_A,servo_B);
         alvik.setServoA(servo_A);
diff --git a/examples/kinematics/kinematics.ino b/examples/kinematics/kinematics.ino
index 48e526a..056c93b 100644
--- a/examples/kinematics/kinematics.ino
+++ b/examples/kinematics/kinematics.ino
@@ -31,11 +31,16 @@ void loop() {
   if (millis()-tmotor>20){
     tmotor=millis();
     alvik.updateMotors();
-    alvik.kinematics->updatePose(alvik.motor_control_left->getTravel(), alvik.motor_control_right->getTravel());
+    alvik.kinematics->inverse(alvik.motor_control_left->getRPM(),alvik.motor_control_right->getRPM());
+    alvik.kinematics->updatePose();
     Serial.print("\t");
-    Serial.print(alvik.kinematics->getDeltaX());
+    Serial.print(alvik.kinematics->getLinearVelocity());
     Serial.print("\t");
-    Serial.print(alvik.kinematics->getDeltaY());
+    Serial.print(alvik.kinematics->getAngularVelocity());
+    Serial.print("\t");
+    Serial.print(alvik.kinematics->getX());
+    Serial.print("\t");
+    Serial.print(alvik.kinematics->getY());
     Serial.print("\t");
     Serial.print(alvik.kinematics->getTheta());
     Serial.print("\n");
@@ -45,16 +50,16 @@ void loop() {
     ttask=millis();
     switch (task){
       case 0:
-        alvik.drive(50,0);
+        alvik.rotate(90);
         break;
       case 1:
-        alvik.drive(0,0);
+        alvik.drive(40,0);
         break;
       case 2:
-        alvik.drive(0,-90);
+        alvik.rotate(-90);
         break;
       case 3:
-        alvik.drive(0,0);
+        alvik.drive(-40,0);
         break;
     }
     task++;
diff --git a/examples/position/position.ino b/examples/position/position.ino
new file mode 100644
index 0000000..68a3669
--- /dev/null
+++ b/examples/position/position.ino
@@ -0,0 +1,83 @@
+/*
+    This file is part of the Arduino_AlvikCarrier library.
+
+    Copyright (c) 2023 Arduino SA
+
+    This Source Code Form is subject to the terms of the Mozilla Public
+    License, v. 2.0. If a copy of the MPL was not distributed with this
+    file, You can obtain one at http://mozilla.org/MPL/2.0/.
+    
+*/
+
+
+#include "Arduino_AlvikCarrier.h"
+
+Arduino_AlvikCarrier alvik;
+
+unsigned long tmotor=0;
+unsigned long ttask=0;
+uint8_t task=0;
+float reference;
+
+void setup() {
+  Serial.begin(115200);
+  alvik.begin();
+  task = 0;
+  reference = 0;
+  ttask = millis();
+  tmotor = millis();
+}
+
+void loop() {
+  if (millis()-tmotor>20){
+    tmotor = millis();
+    alvik.updateMotors();
+    Serial.print(reference);
+    Serial.print("\t");
+    Serial.print(alvik.getPositionLeft());
+    Serial.print("\n");
+  }
+
+  if (millis()-ttask>5000){
+    ttask = millis();
+    switch (task){
+      case 0:
+        reference = 90;
+        break;
+      case 1:
+        reference = 0;
+        break;
+      case 2:
+        reference = -90;
+        break;
+      case 3:
+        reference = 0;
+        break;
+      case 4:
+        reference = 360;
+        break;
+      case 5:
+        reference = 45;
+        break;
+      case 6:
+        reference = -270;
+        break;
+      case 7:
+        reference = 0;
+        break;
+      case 8:
+        reference = 5;
+        break;
+      case 9:
+        reference = 10;
+        break;
+    }
+    alvik.setPositionLeft(reference);
+    task++;
+    if (task>9){
+      task = 0;
+    }
+  }
+  
+}
+
diff --git a/src/Arduino_AlvikCarrier.cpp b/src/Arduino_AlvikCarrier.cpp
index 5c1354a..d35fe00 100644
--- a/src/Arduino_AlvikCarrier.cpp
+++ b/src/Arduino_AlvikCarrier.cpp
@@ -19,29 +19,38 @@ Arduino_AlvikCarrier::Arduino_AlvikCarrier(){
     wire = new TwoWire(I2C_1_SDA, I2C_1_SCL);
 
     // I2C external bus
-    ext_wire = new TwoWire(I2C_2_SDA,I2C_2_SCL);
+    ext_wire = new TwoWire(I2C_2_SDA, I2C_2_SCL);
 
     // uart to esp32
-    serial = new HardwareSerial(UART_RX,UART_TX);
+    serial = new HardwareSerial(UART_RX, UART_TX);
 
     // RGB leds
-    led1 = new RGBled(LED_1_RED,LED_1_GREEN,LED_1_BLUE);
-    led2 = new RGBled(LED_2_RED,LED_2_GREEN,LED_2_BLUE);
+    led1 = new RGBled(LED_1_RED, LED_1_GREEN, LED_1_BLUE);
+    led2 = new RGBled(LED_2_RED, LED_2_GREEN, LED_2_BLUE);
 
     // motors
-    motor_left = new DCmotor(MOTOR_LEFT_A,MOTOR_LEFT_A_CH, MOTOR_LEFT_B, MOTOR_LEFT_B_CH,MOTOR_LEFT_FLIP);
-    motor_right = new DCmotor(MOTOR_RIGHT_A,MOTOR_RIGHT_A_CH,MOTOR_RIGHT_B,MOTOR_RIGHT_B_CH,MOTOR_RIGHT_FLIP);
+    motor_left = new DCmotor(MOTOR_LEFT_A, MOTOR_LEFT_A_CH, MOTOR_LEFT_B, MOTOR_LEFT_B_CH, MOTOR_LEFT_FLIP);
+    motor_right = new DCmotor(MOTOR_RIGHT_A, MOTOR_RIGHT_A_CH, MOTOR_RIGHT_B, MOTOR_RIGHT_B_CH, MOTOR_RIGHT_FLIP);
 
     // encoders
-    encoder_left = new Encoder(TIM3,ENC_LEFT_FLIP);
-    encoder_right = new Encoder(TIM5,ENC_RIGHT_FLIP);
+    encoder_left = new Encoder(TIM3, ENC_LEFT_FLIP);
+    encoder_right = new Encoder(TIM5, ENC_RIGHT_FLIP);
 
     // motor control
-    motor_control_left = new MotorControl(motor_left,encoder_left,MOTOR_KP_DEFAULT,MOTOR_KI_DEFAULT,MOTOR_KD_DEFAULT,MOTOR_CONTROL_PERIOD);
-    motor_control_right = new MotorControl(motor_right,encoder_right,MOTOR_KP_DEFAULT,MOTOR_KI_DEFAULT,MOTOR_KD_DEFAULT,MOTOR_CONTROL_PERIOD);
+    motor_control_left = new MotorControl(motor_left, encoder_left,
+                                          MOTOR_KP_DEFAULT, MOTOR_KI_DEFAULT, MOTOR_KD_DEFAULT,
+                                          MOTOR_CONTROL_PERIOD, CONTROL_MODE_LINEAR, MOTOR_CONTROL_STEP,
+                                          POSITION_KP_DEFAULT, POSITION_KI_DEFAULT, POSITION_KD_DEFAULT,
+                                          POSITION_CONTROL_PERIOD, POSITION_MAX_SPEED);
+
+    motor_control_right = new MotorControl(motor_right, encoder_right, 
+                                          MOTOR_KP_DEFAULT, MOTOR_KI_DEFAULT, MOTOR_KD_DEFAULT,
+                                          MOTOR_CONTROL_PERIOD, CONTROL_MODE_LINEAR, MOTOR_CONTROL_STEP,
+                                          POSITION_KP_DEFAULT, POSITION_KI_DEFAULT, POSITION_KD_DEFAULT,
+                                          POSITION_CONTROL_PERIOD, POSITION_MAX_SPEED);
 
     // color sensor
-    apds9960 = new APDS9960(*wire,APDS_INT);
+    apds9960 = new APDS9960(*wire, APDS_INT);
 
     // servo
     servo_A = new Servo();
@@ -57,7 +66,7 @@ Arduino_AlvikCarrier::Arduino_AlvikCarrier(){
     imu = new LSM6DSOSensor(wire, LSM6DSO_I2C_ADD_L);
     ipKnobs = &iKnobs;
     imu_delta_time = MOTION_FX_ENGINE_DELTATIME;
-    sample_to_discard=0;
+    sample_to_discard = 0;
 
     // version
     version_high = VERSION_BYTE_HIGH;
@@ -65,7 +74,8 @@ Arduino_AlvikCarrier::Arduino_AlvikCarrier(){
     version_low = VERSION_BYTE_LOW;
 
     // kinematics
-    kinematics = new Kinematics(WHEEL_TRACK_MM,WHEEL_DIAMETER_MM);
+    kinematics = new Kinematics(WHEEL_TRACK_MM, WHEEL_DIAMETER_MM);
+    kinematics_movement = 0;
 
 }
 
@@ -124,9 +134,9 @@ int Arduino_AlvikCarrier::begin(){
 }
 
 void Arduino_AlvikCarrier::getVersion(uint8_t &high_byte, uint8_t &mid_byte, uint8_t &low_byte){
-    high_byte=version_high;
-    mid_byte=version_mid;
-    low_byte=version_low;
+    high_byte = version_high;
+    mid_byte = version_mid;
+    low_byte = version_low;
 }
 
 
@@ -135,7 +145,7 @@ void Arduino_AlvikCarrier::getVersion(uint8_t &high_byte, uint8_t &mid_byte, uin
 /******************************************************************************************************/
 
 int Arduino_AlvikCarrier::beginAPDS(){
-    pinMode(APDS_LED,OUTPUT);
+    pinMode(APDS_LED, OUTPUT);
     enableIlluminator();
     if (!apds9960->begin()){
         return ERROR_APDS;
@@ -149,13 +159,13 @@ void Arduino_AlvikCarrier::updateAPDS(){
     }
     //digitalWrite(APDS_LED,HIGH);
     if (apds9960->colorAvailable()){
-        apds9960->readColor(bottom_red,bottom_green,bottom_blue,bottom_clear);
+        apds9960->readColor(bottom_red, bottom_green, bottom_blue, bottom_clear);
     }
     //digitalWrite(APDS_LED,LOW);
 }
 
 void Arduino_AlvikCarrier::setIlluminator(uint8_t value){
-    digitalWrite(APDS_LED,value);
+    digitalWrite(APDS_LED, value);
 }
 
 void Arduino_AlvikCarrier::enableIlluminator(){
@@ -207,11 +217,11 @@ void Arduino_AlvikCarrier::setServoB(int position){
 /******************************************************************************************************/
 
 int Arduino_AlvikCarrier::beginI2Cselect(){
-    pinMode(SELECT_I2C_BUS,OUTPUT);
+    pinMode(SELECT_I2C_BUS, OUTPUT);
 }
 
 void Arduino_AlvikCarrier::setExternalI2C(uint8_t state){
-    digitalWrite(SELECT_I2C_BUS,state);
+    digitalWrite(SELECT_I2C_BUS, state);
 }
 
 void Arduino_AlvikCarrier::connectExternalI2C(){
@@ -313,6 +323,53 @@ void Arduino_AlvikCarrier::setKPidLeft(const float kp, const float ki, const flo
     motor_control_left->setKD(kd);
 }
 
+void Arduino_AlvikCarrier::setPositionLeft(const float degrees){
+    motor_control_left->setPosition(degrees);
+}
+
+float Arduino_AlvikCarrier::getPositionLeft(){
+    return motor_control_left->getPosition();
+}
+
+void Arduino_AlvikCarrier::setPositionRight(const float degrees){
+    motor_control_right->setPosition(degrees);
+}  
+
+float Arduino_AlvikCarrier::getPositionRight(){
+    return motor_control_right->getPosition();
+}
+
+void Arduino_AlvikCarrier::setPosition(const float left_deg, const float right_deg){
+    setPositionLeft(left_deg);
+    setPositionRight(right_deg);
+}
+
+void Arduino_AlvikCarrier::getPosition(float & left_deg, float & right_deg){
+    left_deg = getPositionLeft();
+    right_deg = getPositionRight();
+}
+
+void Arduino_AlvikCarrier::resetPositionLeft(const float initial_position){
+    motor_control_left->resetPosition(initial_position);
+}
+
+void Arduino_AlvikCarrier::resetPositionRight(const float initial_position){
+    motor_control_right->resetPosition(initial_position);
+}
+
+void Arduino_AlvikCarrier::disablePositionControlLeft(){
+    motor_control_left->disablePositionControl();
+}
+
+void Arduino_AlvikCarrier::disablePositionControlRight(){
+    motor_control_right->disablePositionControl();
+}
+
+void Arduino_AlvikCarrier::disablePositionControl(){
+    disablePositionControlLeft();
+    disablePositionControlRight();
+} 
+
 
 
 /******************************************************************************************************/
@@ -347,14 +404,14 @@ void Arduino_AlvikCarrier::updateTouch(){
 }
 
 bool Arduino_AlvikCarrier::getAnyTouchPressed(){
-    if (touch_sensor->touched(touch_status,TOUCH_PAD_GUARD)){
+    if (touch_sensor->touched(touch_status, TOUCH_PAD_GUARD)){
         return true;
     }
     return false;
 }
 
 bool Arduino_AlvikCarrier::getTouchKey(const uint8_t key){
-    if (touch_sensor->touched(touch_status,key)&&touch_sensor->touched(touch_status,TOUCH_PAD_GUARD)){
+    if (touch_sensor->touched(touch_status, key)&&touch_sensor->touched(touch_status, TOUCH_PAD_GUARD)){
         return true;
     }
     return false;
@@ -410,7 +467,7 @@ bool Arduino_AlvikCarrier::getTouchDelete(){
 /******************************************************************************************************/
 
 int Arduino_AlvikCarrier::beginLeds(){
-    pinMode(LED_BUILTIN,OUTPUT);
+    pinMode(LED_BUILTIN, OUTPUT);
     // turn off leds
     led1->clear();
     led2->clear();
@@ -418,7 +475,7 @@ int Arduino_AlvikCarrier::beginLeds(){
 }
 
 void Arduino_AlvikCarrier::setLedBuiltin(const uint8_t value){
-    digitalWrite(LED_BUILTIN,value);
+    digitalWrite(LED_BUILTIN, value);
 }
 
 void Arduino_AlvikCarrier::setLedLeft(const uint32_t color){
@@ -426,7 +483,7 @@ void Arduino_AlvikCarrier::setLedLeft(const uint32_t color){
 }
 
 void Arduino_AlvikCarrier::setLedLeft(const uint32_t red, const uint32_t green, const uint32_t blue){
-    led1->set(red,green,blue); 
+    led1->set(red, green, blue); 
 }
 
 void Arduino_AlvikCarrier::setLedLeftRed(const uint32_t red){
@@ -446,7 +503,7 @@ void Arduino_AlvikCarrier::setLedRight(const uint32_t color){
 }
 
 void Arduino_AlvikCarrier::setLedRight(const uint32_t red, const uint32_t green, const uint32_t blue){
-    led2->set(red,green,blue);
+    led2->set(red, green, blue);
 }
 
 void Arduino_AlvikCarrier::setLedRightRed(const uint32_t red){
@@ -467,8 +524,8 @@ void Arduino_AlvikCarrier::setLeds(const uint32_t color){
 }
 
 void Arduino_AlvikCarrier::setLeds(const uint32_t red, const uint32_t green, const uint32_t blue){
-    setLedLeft(red,green,blue);
-    setLedRight(red,green,blue);    
+    setLedLeft(red, green, blue);
+    setLedRight(red, green, blue);    
 }
 
 void Arduino_AlvikCarrier::setAllLeds(const uint8_t value){
@@ -603,31 +660,78 @@ void Arduino_AlvikCarrier::errorLed(const int error_code){
 /*                                             Kinematics                                             */
 /******************************************************************************************************/
 
+
+
+
+
+
+
+
 void Arduino_AlvikCarrier::drive(const float linear, const float angular){
     kinematics->forward(linear, angular);
-    setRpm(kinematics->getLeftVelocity(),kinematics->getRightVelocity());
+    setRpm(kinematics->getLeftVelocity(), kinematics->getRightVelocity());
 }
 
 void Arduino_AlvikCarrier::rotate(const float angle){
-    float initial_angle=kinematics->getTheta();
-    float error=angle-initial_angle;
-    unsigned long t=millis();
+    float initial_angle = kinematics->getTheta();
+    float final_angle = angle+initial_angle;
+    float error = angle;
+    unsigned long t = millis();
     while(abs(error)>2){
         if (millis()-t>20){
-            t=millis();
+            t = millis();
             updateMotors();
-            kinematics->updatePose(motor_control_left->getAngle(),motor_control_right->getAngle());
-            error=angle-kinematics->getTheta();
-            Serial.println(error);
+            kinematics->inverse(motor_control_left->getRPM(),motor_control_right->getRPM());
+            kinematics->updatePose();
+            error = final_angle-kinematics->getTheta();
         }
-        if (error>0){
-            drive(0,40);
+        if (angle>0){
+            drive(0, 40);
         }else{
-            drive(0,-40);
+            drive(0, -40);
         }
-
     }
     drive(0,0);
     updateMotors();
+    motor_control_left->brake();
+    motor_control_right->brake();
+}
+
+
+
+void Arduino_AlvikCarrier::move(const float distance){
+    float initial_travel = kinematics->getTravel();
+    float final_travel = abs(distance)+initial_travel;
+    float error = abs(distance);
+    unsigned long t = millis();
+
+    while(error>2){
+        if (millis()-t>20){
+            t = millis();
+            updateMotors();
+            kinematics->inverse(motor_control_left->getRPM(), motor_control_right->getRPM());
+            kinematics->updatePose();
+            error = final_travel-kinematics->getTravel();
+        }
+        if (distance>0){
+            drive(40, 0);
+        }else{
+            drive(-40, 0);
+        }
+    }
+    drive(0, 0);
+    updateMotors();
+    motor_control_left->brake();
+    motor_control_right->brake();
 }
 
+
+
+
+void Arduino_AlvikCarrier::updateKinematics(){
+    kinematics->inverse(motor_control_left->getRPM(), motor_control_right->getRPM());
+    kinematics->updatePose();
+    if (kinematics_movement!=0){
+        // do controls
+    }
+}
diff --git a/src/Arduino_AlvikCarrier.h b/src/Arduino_AlvikCarrier.h
index 6941245..e6645bc 100644
--- a/src/Arduino_AlvikCarrier.h
+++ b/src/Arduino_AlvikCarrier.h
@@ -69,6 +69,9 @@ class Arduino_AlvikCarrier{
         uint8_t version_low;
 
 
+        uint8_t kinematics_movement;
+
+
 
     public:        
         Kinematics * kinematics;
@@ -139,8 +142,20 @@ class Arduino_AlvikCarrier{
         float getRpmRight();                                                            // get RPM of right motor
         bool setRpm(const float left, const float right);                               // sets RPMs on left and right wheels
         void getRpm(float & left, float & right);                                       // get RPMs on left and right wheels
-        void setKPidRight(const float kp, const float ki, const float kd);              // set PID parameters for right wheel
         void setKPidLeft(const float kp, const float ki, const float kd);               // set PID parameters for left wheel
+        void setKPidRight(const float kp, const float ki, const float kd);              // set PID parameters for right wheel
+        void setPositionLeft(const float degrees);                                      // set position in degrees on left wheel
+        float getPositionLeft();                                                        // get left wheel position in degrees
+        void setPositionRight(const float degrees);                                     // set position in degrees on right wheel
+        float getPositionRight();                                                       // get right wheel position in degrees
+        void setPosition(const float left_deg, const float right_deg);                  // set positions on both wheels
+        void getPosition(float & left_deg, float & right_deg);                          // get both wheels position
+        void resetPositionLeft(const float initial_position=0.0);                       // reset/set value of position for left wheel
+        void resetPositionRight(const float initial_position=0.0);                      // reset/set value of position for right wheel
+        void disablePositionControlLeft();                                              // disable the position control on left wheel
+        void disablePositionControlRight();                                             // disable the position control on right wheel
+        void disablePositionControl();                                                  // disable the position control on both wheels
+
 
 
 
@@ -195,9 +210,11 @@ class Arduino_AlvikCarrier{
 
 
         // Kinematics
+        void updateKinematics();
         void drive(const float linear, const float angular);                            // set mm/s and deg/s of the robot
 
-        void rotate(const float angle);
+        void move(const float distance);                                                // move of distance millimeters
+        void rotate(const float angle);                                                 // rotate of angle degrees
 
         
 
diff --git a/src/definitions/robot_definitions.h b/src/definitions/robot_definitions.h
index 6bb1796..5c5e6ae 100644
--- a/src/definitions/robot_definitions.h
+++ b/src/definitions/robot_definitions.h
@@ -25,10 +25,14 @@ const float MOTOR_RATIO = MOTOR_CPR*MOTOR_GEAR_RATIO;
 #define MOTOR_KP_DEFAULT 32.0
 #define MOTOR_KI_DEFAULT 450.0
 #define MOTOR_KD_DEFAULT 0.0
-
-//30, 450, 0.0
-//120,300,1,0
 #define MOTOR_CONTROL_PERIOD 0.02
+#define MOTOR_CONTROL_STEP 5.0
+
+#define POSITION_KP_DEFAULT 1.0
+#define POSITION_KI_DEFAULT 0.0
+#define POSITION_KD_DEFAULT 0.0001
+#define POSITION_CONTROL_PERIOD 0.02
+#define POSITION_MAX_SPEED 30.0
 
 
 // Sensor fusioning parameters
diff --git a/src/motor_control/motor_control.cpp b/src/motor_control/motor_control.cpp
index 72a1d6f..960552d 100644
--- a/src/motor_control/motor_control.cpp
+++ b/src/motor_control/motor_control.cpp
@@ -13,7 +13,10 @@
 
 MotorControl::MotorControl(DCmotor * _motor, Encoder * _encoder, const float _kp, const float _ki, const float _kd,
                                        const float _controller_period,
-                                       const uint8_t _control_mode, const float _step_size){
+                                       const uint8_t _control_mode, const float _step_size,
+                                       const float _kp_pos, const float _ki_pos, const float _kd_pos,
+                                       const float _pos_controller_period,
+                                       const float _pos_max_velocity){
     motor = _motor;
     encoder = _encoder;
     
@@ -26,28 +29,39 @@ MotorControl::MotorControl(DCmotor * _motor, Encoder * _encoder, const float _kp
 
     controller_period = _controller_period;
 
-    travel=0.0;
-    angle=0.0;
+    kp_pos = _kp_pos;
+    ki_pos = _ki_pos;
+    kd_pos = _kd_pos;
+    pos_controller_period = _pos_controller_period;
+    pos_max_velocity = _pos_max_velocity;
+    position_control_enabled = false;
+
+
+
+    position = 0.0;
+    angle = 0.0;
     reference = 0.0;
 
-    trip=0.0;
-    iterations=0.0;
-    start_value=0.0;
-    end_value=0.0;
-    step_size=_step_size;
-    step=0.0;
-    step_index=0;
-    interpolation=0.0;
+    trip = 0.0;
+    iterations = 0.0;
+    start_value = 0.0;
+    end_value = 0.0;
+    step_size = _step_size;
+    step = 0.0;
+    step_index = 0;
+    interpolation = 0.0;
 
     measure = 0.0;
     last_measure = 0.0;
 
-    id_memory=0;
-    mean=0.0;
+    id_memory = 0;
+    mean = 0.0;
 
-    conversion_factor_travel = (1.0/MOTOR_RATIO);
+    conversion_factor_angle = (1.0/MOTOR_RATIO)*360.0;
     conversion_factor = 60.0*(1.0/MOTOR_RATIO)/(controller_period);
+
     vel_pid = new PidController(kp,ki,kd,controller_period,CONTROL_LIMIT);
+    pos_pid = new PidController(kp_pos, ki_pos, kd_pos, pos_controller_period, pos_max_velocity);
 }
 
 void MotorControl::begin(){
@@ -55,6 +69,7 @@ void MotorControl::begin(){
     encoder->begin();
     encoder->reset();
     vel_pid->reset();
+    pos_pid->reset();
     clearMemory();
 }
 
@@ -118,38 +133,19 @@ void MotorControl::update(){
 
     measure = encoder->getCount();
     encoder->reset();
-    angle = measure*conversion_factor_travel;
-    travel += angle;
+    angle = measure*conversion_factor_angle;
+    position+=angle;
     measure = measure*conversion_factor;
-
-    /* experimental
-    if (abs(measure)-abs(reference)>5){
-        clearMemory(reference);  
-    }
-    end */
-
     addMemory(measure);
-
-    /*
-    if (abs(reference)<1.0){
-        vel_pid->reset();
-        motor->setSpeed(0);
-        clearMemory();
-    }
-    */
-
     measure = meanMemory();
 
-    //vel_pid->update(measure);
-    //motor->setSpeed(vel_pid->getControlOutput());
-
     if (control_mode==CONTROL_MODE_LINEAR){
         if (step_index<iterations){
             step_index++;
             trip+=step;
-            interpolation=trip*(end_value-start_value)+start_value;
+            interpolation = trip*(end_value-start_value)+start_value;
             if (abs(interpolation)>abs(reference)){
-                interpolation=reference;
+                interpolation = reference;
             }
         }
         vel_pid->setReference(interpolation);
@@ -160,6 +156,11 @@ void MotorControl::update(){
 
     vel_pid->update(measure);
     motor->setSpeed(vel_pid->getControlOutput());
+
+    if (position_control_enabled){
+        pos_pid->update(position);
+        setRPM(round(pos_pid->getControlOutput()/10.0)*10);
+    }
 }
 
 void MotorControl::setKP(const float _kp){
@@ -179,14 +180,14 @@ float MotorControl::getError(){
 }
 
 void MotorControl::brake(){
-    reference=0.0;
-    trip=0.0;
-    iterations=0.0;
-    start_value=0.0;
-    end_value=0.0;
-    step=0.0;
-    step_index=0;
-    interpolation=0.0;
+    reference = 0.0;
+    trip = 0.0;
+    iterations = 0.0;
+    start_value = 0.0;
+    end_value = 0.0;
+    step = 0.0;
+    step_index = 0;
+    interpolation = 0.0;
     clearMemory();
 
     motor->setSpeed(0);
@@ -194,14 +195,29 @@ void MotorControl::brake(){
     vel_pid->reset();
 }
 
-void MotorControl::resetTravel(){
-    travel=0;
+
+void MotorControl::enablePositionControl(){
+    position_control_enabled = true;
+}
+
+void MotorControl::disablePositionControl(){
+    position_control_enabled = false;
+}
+
+bool MotorControl::isPositionControlEnabled(){
+    return position_control_enabled;
+}
+
+void MotorControl::resetPosition(const float p0){
+    position=p0;
+}
+
+float MotorControl::getPosition(){
+    return position;
 }
 
-float MotorControl::getTravel(){
-    return travel;
+void MotorControl::setPosition(const float degree){
+    pos_pid->setReference(degree);
+    enablePositionControl();
 }
 
-float MotorControl::getAngle(){
-    return angle;
-}
\ No newline at end of file
diff --git a/src/motor_control/motor_control.h b/src/motor_control/motor_control.h
index 9998fa6..4be149b 100644
--- a/src/motor_control/motor_control.h
+++ b/src/motor_control/motor_control.h
@@ -33,10 +33,20 @@ class MotorControl{
         float kp;
         float ki;
         float kd;
+        float controller_period;
+
+        float kp_pos;
+        float ki_pos;
+        float kd_pos;
+        float pos_controller_period;
+        float pos_max_velocity;
+        bool position_control_enabled;
         
-        float travel;
+        float position;
         float angle;
         float reference;
+        float reference_position;
+
 
         float trip;
         float iterations;
@@ -47,41 +57,42 @@ class MotorControl{
         int step_index;
         float interpolation;
 
-        float controller_period;
         float conversion_factor;
-        float conversion_factor_travel;
+        float conversion_factor_angle;
         float measure;
         float last_measure;
 
-
         float mean;
         int i;
         int id_memory;
         float measure_memory[MEM_SIZE];
 
-
         DCmotor * motor;
         Encoder * encoder;
+
+
     public:
 
         PidController * vel_pid;
+        PidController * pos_pid;
 
-        MotorControl(DCmotor * _motor, Encoder * _encoder, const float _kp, const float _ki, const float _kd,
+        MotorControl(DCmotor * _motor, Encoder * _encoder,
+                                       const float _kp, const float _ki, const float _kd,
                                        const float _controller_period,
-                                       const uint8_t _control_mode = CONTROL_MODE_LINEAR, const float _step_size=5.0);
+                                       const uint8_t _control_mode = CONTROL_MODE_LINEAR, const float _step_size=5.0,
+                                       const float _kp_pos=0.0, const float _ki_pos=0.0, const float _kd_pos=0.0,
+                                       const float _pos_controller_period=0.0,
+                                       const float _pos_max_velocity=0.0
+                                    );
 
         void begin();
 
         float checkLimits(float value);
-
         void addMemory(float _val);
-
         float meanMemory();               
-
         void clearMemory(const float reset_value=0.0);
 
         float getRPM();
-
         bool setRPM(const float ref);
 
         void update();
@@ -90,16 +101,17 @@ class MotorControl{
         void setKI(const float _ki);
         void setKD(const float _kd);
 
-        float getError();
 
         void brake();
 
-        void resetTravel();
-
-        float getTravel();
-
-        float getAngle();
+        void enablePositionControl();
+        void disablePositionControl();
+        bool isPositionControlEnabled();
+        void setPosition(const float degree);                          // set the reference for position control
+        float getPosition();                                            // get the actual angle in degrees of motor
+        void  resetPosition(const float p0=0.0);                        // reset/set the position value
 
+        float getError();
         
 };
 
diff --git a/src/robotics/kinematics.h b/src/robotics/kinematics.h
index ced4bb9..d1558aa 100644
--- a/src/robotics/kinematics.h
+++ b/src/robotics/kinematics.h
@@ -13,6 +13,7 @@
 #define __KINEMATICS_H__
 
 #include "Arduino.h"
+#include "../definitions/robot_definitions.h"
 
 class Kinematics{
     private:
@@ -24,13 +25,9 @@ class Kinematics{
         float wheel_diameter;
         float wheel_radius;
         float wheel_circumference;
-        float rads_to_rpm;
-        float rpm_to_rads;
-        float rads_to_degs;
-        float degs_to_rads;
-        float rotations_to_rads;
-        float rads_to_rotations;
-        float w;
+
+        float v, w;
+        float left_vel, right_vel;
 
         float theta, delta_theta;
         float x, y;
@@ -39,28 +36,55 @@ class Kinematics{
         float delta_x, delta_y;
         float travel;
 
+        float control_period;
+
+
+        float mm_to_m(const float mm){
+            return mm*0.001;
+        }
+
+        float m_to_mm(const float m){
+            return m*1000.0;
+        }
+
+        float rads_to_degs(const float rads){
+            return rads*180.0/PI;
+        }
+
+        float degs_to_rads(const float degs){
+            return degs*PI/180.0;
+        }
+
+        float rads_to_rpm(const float rads){
+            return rads*60.0/(2.0*PI);
+        }
+
+        float rpm_to_rads(const float rpm){
+            return rpm*2.0*PI/60.0;
+        }
+
+        float rads_to_rotations(const float rads){
+            return rads/(2.0*PI);
+        }
+
+        float rotations_to_rads(const float rotations){
+            return rotations*2.0*PI;
+        }
 
 
     public:
-        Kinematics(const float _wheel_track, const float _wheel_diameter){
+        Kinematics(const float _wheel_track, const float _wheel_diameter, const float _control_period=MOTOR_CONTROL_PERIOD){
             w=0.0;
             left_velocity=0.0;
             right_velocity=0.0;
             linear_velocity=0.0;
             angular_velocity=0.0;
-            wheel_track=_wheel_track;
-            wheel_diameter=_wheel_diameter;
-            wheel_radius=wheel_diameter/2.0;
-            wheel_circumference=wheel_diameter*PI;
-
-            rads_to_rpm=60.0/(2.0*PI);
-            rpm_to_rads=2.0*PI/60.0;
 
-            rads_to_degs=180.0/PI;
-            degs_to_rads=PI/180.0;
+            wheel_track=mm_to_m(_wheel_track);
+            wheel_diameter=mm_to_m(_wheel_diameter);
 
-            rads_to_rotations=1/(2.0*PI);
-            rotations_to_rads=2.0*PI;
+            wheel_radius=wheel_diameter/2.0;
+            wheel_circumference=wheel_diameter*PI;
 
             theta=0.0;
             x=0.0;
@@ -74,44 +98,65 @@ class Kinematics{
             delta_travel=0.0;
             travel=0.0;
 
+            control_period = _control_period;
         }
 
         void forward(const float linear, const float angular){
-            w = angular*degs_to_rads;
-            left_velocity=(2*linear-w*wheel_track)/(wheel_diameter);
-            left_velocity=rads_to_rpm*left_velocity;
-            right_velocity=(2*linear+w*wheel_track)/(wheel_diameter);
-            right_velocity=rads_to_rpm*right_velocity;
+            v = mm_to_m(linear);
+            w = degs_to_rads(angular);
+
+            left_velocity=(2*v-w*wheel_track)/(wheel_diameter);
+            right_velocity=(2*v+w*wheel_track)/(wheel_diameter);
         }
 
-        void inverse(const float left_vel, const float right_vel){
+        void inverse(const float left, const float right){
+            left_vel=rpm_to_rads(left);
+            right_vel=rpm_to_rads(right);
+
             linear_velocity=(left_vel+right_vel)*wheel_radius/2.0;
             angular_velocity=(-left_vel+right_vel)*wheel_radius/wheel_track;
         }
 
         float getLeftVelocity(){
-            return left_velocity;
+            return rads_to_rpm(left_velocity);
         }
 
         float getRightVelocity(){
-            return right_velocity;
+            return rads_to_rpm(right_velocity);
         }
 
         float getLinearVelocity(){
-            return linear_velocity;
+            return m_to_mm(linear_velocity);
         }
 
         float getAngularVelocity(){
-            return angular_velocity;
+            return rads_to_degs(angular_velocity);
         }
 
+        /*
         void updatePose(const float left_rotation, const float right_rotation){
             delta_left=left_rotation*wheel_circumference;
             delta_right=right_rotation*wheel_circumference;
             delta_travel=(delta_left+delta_right)/2.0;
-            delta_theta=(-delta_left+delta_right)/(2.0*wheel_track);
+            delta_theta=(-delta_left+delta_right)/(wheel_track);
+            
             delta_x=delta_travel*cos(theta+delta_theta/2.0);
             delta_y=delta_travel*sin(theta+delta_theta/2.0);
+            
+            delta_x=delta_travel*cos(delta_theta);
+            delta_y=delta_travel*sin(delta_theta);
+            x+=delta_x;
+            y+=delta_y;
+            theta+=delta_theta;
+            travel+=delta_travel; 
+        }
+        */
+
+        void updatePose(){
+            delta_theta=angular_velocity*control_period;
+            delta_x=linear_velocity*cos(theta)*control_period;
+            delta_y=linear_velocity*sin(theta)*control_period;
+            delta_travel=sqrt(delta_x*delta_x+delta_y*delta_y);
             x+=delta_x;
             y+=delta_y;
             theta+=delta_theta;
@@ -119,9 +164,9 @@ class Kinematics{
         }
 
         void resetPose(const float initial_x=0.0, const float initial_y=0.0, const float initial_theta=0.0){
-            x=initial_x;
-            y=initial_y;
-            theta=degs_to_rads*initial_theta;
+            x=mm_to_m(initial_x);
+            y=mm_to_m(initial_y);
+            theta=degs_to_rads(initial_theta);
             travel=0.0;
             delta_x=0.0;
             delta_y=0.0;
@@ -133,31 +178,31 @@ class Kinematics{
         }
 
         float getX(){
-            return x;
+            return m_to_mm(x);
         }
 
         float getY(){
-            return y;
+            return m_to_mm(y);
         }
 
         float getTheta(){
-            return rads_to_degs*theta;
+            return rads_to_degs(theta);
         }
 
         float getTravel(){
-            return travel;
+            return m_to_mm(travel);
         }
 
         float getDeltaX(){
-            return delta_x;
+            return m_to_mm(delta_x);
         }
 
         float getDeltaY(){
-            return delta_y;
+            return m_to_mm(delta_y);
         }
 
         float getDeltaTheta(){
-            return rads_to_degs*delta_theta;
+            return rads_to_degs(delta_theta);
         }