diff --git a/examples/firmware_01/firmware_01.ino b/examples/firmware_01/firmware_01.ino
index 18c8865..ac678b3 100644
--- a/examples/firmware_01/firmware_01.ino
+++ b/examples/firmware_01/firmware_01.ino
@@ -9,6 +9,8 @@
     
 */
 
+// WIP -> preliminary firmware
+
 
 #include "Arduino_AlvikCarrier.h"
 #include "sensor_line.h"
@@ -35,6 +37,7 @@ unsigned long timu=0;
 
 
 float left, right, value;
+float linear, angular;
 uint8_t leds;
 
 uint8_t sensor_id = 0;
@@ -42,6 +45,7 @@ uint8_t sensor_id = 0;
 
 uint8_t pid;
 float kp, ki, kd;
+float x, y, theta;
 
 uint8_t servo_A, servo_B;
 
@@ -49,6 +53,8 @@ uint8_t servo_A, servo_B;
 void setup(){
   Serial.begin(115200);
   alvik.begin();
+  alvik.disableIlluminator();
+  alvik.setLeds(COLOR_ORANGE);
   alvik.setLedBuiltin(HIGH);
   line.begin();
   tof.begin();
@@ -59,6 +65,7 @@ void setup(){
   alvik.serial->write(packeter.msg,msg_size);
 
   alvik.setLedBuiltin(LOW);
+  alvik.setLeds(COLOR_BLACK);
 
 
   code=0;
@@ -77,20 +84,51 @@ void loop(){
     switch (code){
       case 'J':
         packeter.unpacketC2F(code,left,right);
+        alvik.disableKinematicsMovement();
+        alvik.disablePositionControl();
         alvik.setRpm(left, right);
         break;
+
+      case 'V':
+        packeter.unpacketC2F(code,linear,angular);
+        alvik.disableKinematicsMovement();
+        alvik.disablePositionControl();
+        alvik.drive(linear,angular);
+        break;
+
       case 'W':
         packeter.unpacketC2B1F(code,label,control_type,value);
-        if ((label == 'L') && (control_type == 'V')) {
-          alvik.motor_control_left->setRPM(value);
+        alvik.disableKinematicsMovement();
+        if (label=='L'){
+          switch (control_type){
+            case 'V':
+              alvik.disablePositionControlLeft();
+              alvik.setRpmLeft(value);
+              break;
+            case 'P':
+              alvik.setPositionLeft(value);
+              break;
+            case 'Z':
+              alvik.resetPositionLeft(value);
+              break;
+          }
         }
-        else if ((label == 'R') && (control_type == 'V'))
-        {
-          alvik.motor_control_right->setRPM(value);
+        if (label=='R'){
+          switch (control_type){
+            case 'V':
+              alvik.disablePositionControlRight();
+              alvik.setRpmRight(value);
+              break;
+            case 'P':
+              alvik.setPositionRight(value);
+              break;
+            case 'Z':
+              alvik.resetPositionRight(value);
+              break;
+          }
         }
-        
         break;
-
+      
       case 'S':
         packeter.unpacketC2B(code,servo_A,servo_B);
         alvik.setServoA(servo_A);
@@ -111,6 +149,23 @@ void loop(){
           alvik.setKPidRight(kp,ki,kd);
         }
         break;
+
+      case 'R':
+        packeter.unpacketC1F(code, value);
+        alvik.disablePositionControl();
+        alvik.rotate(value);
+        break;
+      
+      case 'G':
+        packeter.unpacketC1F(code, value);
+        alvik.disablePositionControl();
+        alvik.move(value);
+        break;
+
+      case 'Z':
+        packeter.unpacketC3F(code, x, y, theta);
+        alvik.resetPose(x, y, theta);
+        break;
     }
   }
 
@@ -152,9 +207,33 @@ void loop(){
   if (millis()-tmotor>20){
     tmotor=millis();
     alvik.updateMotors();
+    alvik.updateKinematics();
+    // joint speed
     msg_size = packeter.packetC2F('j', alvik.getRpmLeft(),alvik.getRpmRight());
     alvik.serial->write(packeter.msg,msg_size);
-   
+    // joint position
+    msg_size = packeter.packetC2F('w', alvik.getPositionLeft(),alvik.getPositionRight());
+    alvik.serial->write(packeter.msg, msg_size);
+    // robot speed
+    msg_size = packeter.packetC2F('v', alvik.getLinearVelocity(), alvik.getAngularVelocity());
+    alvik.serial->write(packeter.msg, msg_size);
+    // pose
+    msg_size = packeter.packetC3F('s', alvik.getX(), alvik.getY(), alvik.getTheta());
+    alvik.serial->write(packeter.msg, msg_size);
+
+    if (alvik.getKinematicsMovement()!=MOVEMENT_DISABLED){
+      if (alvik.isTargetReached()){
+        if (alvik.getKinematicsMovement()==MOVEMENT_ROTATE){
+          msg_size = packeter.packetC1B('x', 'R');
+        }
+        if (alvik.getKinematicsMovement()==MOVEMENT_MOVE){
+          msg_size = packeter.packetC1B('x', 'M');
+        }
+        alvik.serial->write(packeter.msg, msg_size);
+        //alvik.disableKinematicsMovement();
+      }
+
+    }
   }
 
   if (millis()-timu>10){
@@ -163,4 +242,4 @@ void loop(){
     msg_size = packeter.packetC6F('i', alvik.getAccelerationX(), alvik.getAccelerationY(), alvik.getAccelerationZ(), alvik.getAngularVelocityX(), alvik.getAngularVelocityY(), alvik.getAngularVelocityZ());
     alvik.serial->write(packeter.msg,msg_size);
   }
-}
\ No newline at end of file
+}
diff --git a/examples/kinematics/kinematics.ino b/examples/kinematics/kinematics.ino
index 056c93b..8a595bc 100644
--- a/examples/kinematics/kinematics.ino
+++ b/examples/kinematics/kinematics.ino
@@ -9,7 +9,7 @@
     
 */
 
-// WIP
+// This example shows how to implement kinematics commands and retrieve odometry data from Arduino_AlvikCarrier class
 
 #include "Arduino_AlvikCarrier.h"
 
@@ -17,13 +17,16 @@ Arduino_AlvikCarrier alvik;
 
 unsigned long tmotor=0;
 unsigned long ttask=0;
+unsigned long tled=0;
 uint8_t task=0;
+bool led_value = false;
 
 void setup() {
   Serial.begin(115200);
   alvik.begin();
   ttask=millis();
   tmotor=millis();
+  tled=millis();
   task=0;
 }
 
@@ -31,35 +34,36 @@ void loop() {
   if (millis()-tmotor>20){
     tmotor=millis();
     alvik.updateMotors();
-    alvik.kinematics->inverse(alvik.motor_control_left->getRPM(),alvik.motor_control_right->getRPM());
-    alvik.kinematics->updatePose();
+    alvik.updateKinematics();
     Serial.print("\t");
-    Serial.print(alvik.kinematics->getLinearVelocity());
+    Serial.print(alvik.getLinearVelocity());
     Serial.print("\t");
-    Serial.print(alvik.kinematics->getAngularVelocity());
+    Serial.print(alvik.getAngularVelocity());
     Serial.print("\t");
-    Serial.print(alvik.kinematics->getX());
+    Serial.print(alvik.getX());
     Serial.print("\t");
-    Serial.print(alvik.kinematics->getY());
+    Serial.print(alvik.getY());
     Serial.print("\t");
-    Serial.print(alvik.kinematics->getTheta());
+    Serial.print(alvik.getTheta());
     Serial.print("\n");
   }
 
-  if (millis()-ttask>2000){
+  if ((millis()-ttask>5000)||alvik.isTargetReached()){
     ttask=millis();
     switch (task){
       case 0:
         alvik.rotate(90);
         break;
       case 1:
-        alvik.drive(40,0);
+        alvik.disableKinematicsMovement();
+        alvik.drive(0,0);
         break;
       case 2:
-        alvik.rotate(-90);
+        alvik.move(100);
         break;
       case 3:
-        alvik.drive(-40,0);
+        alvik.disableKinematicsMovement();
+        alvik.drive(0,0);
         break;
     }
     task++;
@@ -67,5 +71,17 @@ void loop() {
       task=0;
     }
   }
-  
+
+  if (millis()-tled>200){
+    tled=millis();
+    led_value=!led_value;
+    if (task==1){
+      alvik.setLedLeftGreen(false);
+      alvik.setLedLeftRed(led_value);
+    }
+    if (task==3){
+      alvik.setLedLeftRed(false);
+      alvik.setLedLeftGreen(led_value);
+    }
+  }
 }
diff --git a/library.properties b/library.properties
index 48d1ddc..9ae0921 100644
--- a/library.properties
+++ b/library.properties
@@ -1,5 +1,5 @@
 name=Arduino Alvik Carrier
-version=0.1.1
+version=0.1.2
 author=Arduino, Giovanni di Dio Bruno, Lucio Rossi
 maintainer=Arduino <info@arduino.cc>
 sentence=Library and firmware for Arduino Alvik Carrier board
diff --git a/src/Arduino_AlvikCarrier.cpp b/src/Arduino_AlvikCarrier.cpp
index d35fe00..e23961c 100644
--- a/src/Arduino_AlvikCarrier.cpp
+++ b/src/Arduino_AlvikCarrier.cpp
@@ -75,8 +75,12 @@ Arduino_AlvikCarrier::Arduino_AlvikCarrier(){
 
     // kinematics
     kinematics = new Kinematics(WHEEL_TRACK_MM, WHEEL_DIAMETER_MM);
-    kinematics_movement = 0;
-
+    kinematics_movement = MOVEMENT_DISABLED;
+    kinematics_achieved = false;
+    previous_travel = 0.0;
+    move_direction = 0.0;
+    rotate_pid = new PidController(ROTATE_KP_DEFAULT, ROTATE_KI_DEFAULT, ROTATE_KD_DEFAULT, ROTATE_CONTROL_PERIOD, ROTATE_MAX_SPEED);
+    move_pid = new PidController(MOVE_KP_DEFAULT, MOVE_KI_DEFAULT, MOVE_KD_DEFAULT, MOVE_CONTROL_PERIOD, MOVE_MAX_SPEED);
 }
 
 int Arduino_AlvikCarrier::begin(){
@@ -146,7 +150,8 @@ void Arduino_AlvikCarrier::getVersion(uint8_t &high_byte, uint8_t &mid_byte, uin
 
 int Arduino_AlvikCarrier::beginAPDS(){
     pinMode(APDS_LED, OUTPUT);
-    enableIlluminator();
+    //enableIlluminator();
+    disableIlluminator();
     if (!apds9960->begin()){
         return ERROR_APDS;
     }
@@ -188,6 +193,10 @@ int Arduino_AlvikCarrier::getBlue(){
     return bottom_blue;
 }
 
+int Arduino_AlvikCarrier::getClear(){
+    return bottom_clear;
+}
+
 int Arduino_AlvikCarrier::getProximity(){
     return bottom_proximity;
 }
@@ -660,19 +669,58 @@ void Arduino_AlvikCarrier::errorLed(const int error_code){
 /*                                             Kinematics                                             */
 /******************************************************************************************************/
 
+void Arduino_AlvikCarrier::updateKinematics(){
+    kinematics->inverse(motor_control_left->getRPM(), motor_control_right->getRPM());
+    kinematics->updatePose();
+    if (kinematics_movement!=MOVEMENT_DISABLED){
+        if (kinematics_movement==MOVEMENT_ROTATE){
+            rotate_pid->update(kinematics->getTheta());
+            drive(0, round(rotate_pid->getControlOutput()/10.0)*10);
+            if (abs(rotate_pid->getError())<ROTATE_THREASHOLD){
+                kinematics_achieved=true;
+            }
+        }
+        if (kinematics_movement==MOVEMENT_MOVE){
+            move_pid->update((kinematics->getTravel()-previous_travel)*move_direction);
+            drive(round(move_pid->getControlOutput()/10.0)*10, 0);
+            if (abs(move_pid->getError())<MOVE_THREADSHOLD){
+                kinematics_achieved=true;
+            }
+            
+        }
+    }
+}
 
+void Arduino_AlvikCarrier::drive(const float linear, const float angular){
+    kinematics->forward(linear, angular);
+    setRpm(kinematics->getLeftVelocity(), kinematics->getRightVelocity());
+}
 
+float Arduino_AlvikCarrier::getLinearVelocity(){
+    return kinematics->getLinearVelocity();
+}
 
+float Arduino_AlvikCarrier::getAngularVelocity(){
+    return kinematics->getAngularVelocity();
+}
 
+float Arduino_AlvikCarrier::getX(){
+    return kinematics->getX();
+}
 
+float Arduino_AlvikCarrier::getY(){
+    return kinematics->getY();
+}
 
+float Arduino_AlvikCarrier::getTheta(){
+    return kinematics->getTheta();
+}
 
-void Arduino_AlvikCarrier::drive(const float linear, const float angular){
-    kinematics->forward(linear, angular);
-    setRpm(kinematics->getLeftVelocity(), kinematics->getRightVelocity());
+void Arduino_AlvikCarrier::resetPose(const float x0, const float y0, const float theta0){
+    kinematics->resetPose(x0,y0,theta0);
 }
 
-void Arduino_AlvikCarrier::rotate(const float angle){
+void Arduino_AlvikCarrier::lockingRotate(const float angle){
     float initial_angle = kinematics->getTheta();
     float final_angle = angle+initial_angle;
     float error = angle;
@@ -697,9 +745,14 @@ void Arduino_AlvikCarrier::rotate(const float angle){
     motor_control_right->brake();
 }
 
+void Arduino_AlvikCarrier::rotate(const float angle){
+    rotate_pid->reset();
+    rotate_pid->setReference(kinematics->getTheta()+angle);
+    kinematics_movement=MOVEMENT_ROTATE;
+    kinematics_achieved=false;
+}
 
-
-void Arduino_AlvikCarrier::move(const float distance){
+void Arduino_AlvikCarrier::lockingMove(const float distance){
     float initial_travel = kinematics->getTravel();
     float final_travel = abs(distance)+initial_travel;
     float error = abs(distance);
@@ -725,13 +778,28 @@ void Arduino_AlvikCarrier::move(const float distance){
     motor_control_right->brake();
 }
 
+void Arduino_AlvikCarrier::move(const float distance){
+    move_pid->reset();
+    previous_travel=kinematics->getTravel();
+    if (distance<0){
+        move_direction=-1.0;
+    }
+    else{
+        move_direction=1.0;
+    }
+    move_pid->setReference(distance);
+    kinematics_movement=MOVEMENT_MOVE;
+    kinematics_achieved=false;
+}
 
+void Arduino_AlvikCarrier::disableKinematicsMovement(){
+    kinematics_movement=MOVEMENT_DISABLED;
+}
 
+bool Arduino_AlvikCarrier::isTargetReached(){
+    return kinematics_achieved;
+}
 
-void Arduino_AlvikCarrier::updateKinematics(){
-    kinematics->inverse(motor_control_left->getRPM(), motor_control_right->getRPM());
-    kinematics->updatePose();
-    if (kinematics_movement!=0){
-        // do controls
-    }
+uint8_t Arduino_AlvikCarrier::getKinematicsMovement(){
+    return kinematics_movement;
 }
diff --git a/src/Arduino_AlvikCarrier.h b/src/Arduino_AlvikCarrier.h
index e6645bc..84cbba1 100644
--- a/src/Arduino_AlvikCarrier.h
+++ b/src/Arduino_AlvikCarrier.h
@@ -70,6 +70,13 @@ class Arduino_AlvikCarrier{
 
 
         uint8_t kinematics_movement;
+        bool kinematics_achieved;
+        float previous_travel;
+        float move_direction;
+        float actual_direction;
+
+        PidController * rotate_pid;
+        PidController * move_pid;
 
 
 
@@ -109,6 +116,7 @@ class Arduino_AlvikCarrier{
         int getRed();                                                                   // red value 0-4095
         int getGreen();                                                                 // green value 0-4095
         int getBlue();                                                                  // blue value 0-4095
+        int getClear();                                                                 // clear value 0-4095
         int getProximity();                                                             // proximity value 0-127
 
 
@@ -210,13 +218,25 @@ class Arduino_AlvikCarrier{
 
 
         // Kinematics
-        void updateKinematics();
+        void updateKinematics();                                                        // update pose/velocity of the robot and controls
         void drive(const float linear, const float angular);                            // set mm/s and deg/s of the robot
+        float getLinearVelocity();                                                      // get linear velocity of the robot in mm/s
+        float getAngularVelocity();                                                     // get angular velocity of the robot in deg/s
+        float getX();                                                                   // absolute position in mm
+        float getY();                                                                   // absolute position in mm
+        float getTheta();                                                               // angle in deg
+        void resetPose(const float x0=0.0, const float y0=0.0, const float theta0=0.0); // reset pose in kinematics
 
         void move(const float distance);                                                // move of distance millimeters
         void rotate(const float angle);                                                 // rotate of angle degrees
-
         
+        
+        void lockingRotate(const float angle);                                          // rotate of angle degrees     
+        void lockingMove(const float distance);                                         // move of distance millimeters
+
+        void disableKinematicsMovement();
+        bool isTargetReached();
+        uint8_t getKinematicsMovement();                                                // get which kind of motion is running in kinematic control
 
 
 };
diff --git a/src/definitions/robot_definitions.h b/src/definitions/robot_definitions.h
index 5c5e6ae..0d742a6 100644
--- a/src/definitions/robot_definitions.h
+++ b/src/definitions/robot_definitions.h
@@ -13,6 +13,10 @@
 #ifndef __ROBOT_DEFINITIONS_H__
 #define __ROBOT_DEFINITIONS_H__
 
+// Robot parameters
+#define WHEEL_TRACK_MM 89.0
+#define WHEEL_DIAMETER_MM 34.0
+
 // Motor Control and mechanical parameters
 #define CONTROL_LIMIT 4096              // PWM resolution
 
@@ -35,6 +39,29 @@ const float MOTOR_RATIO = MOTOR_CPR*MOTOR_GEAR_RATIO;
 #define POSITION_MAX_SPEED 30.0
 
 
+
+// Kinematics control
+#define ROTATE_KP_DEFAULT 5.0
+#define ROTATE_KI_DEFAULT 0.0
+#define ROTATE_KD_DEFAULT 0.001
+#define ROTATE_CONTROL_PERIOD 0.02
+#define ROTATE_MAX_SPEED 45.0
+#define ROTATE_THREASHOLD 3
+
+#define MOVE_KP_DEFAULT 5.0
+#define MOVE_KI_DEFAULT 0.0
+#define MOVE_KD_DEFAULT 0.001
+#define MOVE_CONTROL_PERIOD 0.02
+#define MOVE_MAX_SPEED 45.0
+#define MOVE_THREADSHOLD 3
+
+#define MOVEMENT_DISABLED 0
+#define MOVEMENT_ROTATE 1
+#define MOVEMENT_MOVE 2
+
+
+
+
 // Sensor fusioning parameters
 #define FROM_MG_TO_G  0.001f
 #define FROM_G_TO_MG  1000.0f
@@ -48,15 +75,13 @@ const float MOTION_FX_PERIOD = (1000U / MOTION_FX_FREQ);
 #define GBIAS_ACC_TH_SC                 (2.0f*0.000765f)
 #define GBIAS_GYRO_TH_SC                (2.0f*0.002f)
 #define MOTION_FX_DECIMATION                      1U
-//#define STATE_SIZE                      (size_t)(2432)
 
+
+// Library version
 #define VERSION_BYTE_HIGH 0
 #define VERSION_BYTE_MID 1
-#define VERSION_BYTE_LOW 0
-
+#define VERSION_BYTE_LOW 2
 
 
-#define WHEEL_TRACK_MM 89
-#define WHEEL_DIAMETER_MM 34
 
 #endif
\ No newline at end of file
diff --git a/src/robotics/kinematics.h b/src/robotics/kinematics.h
index d1558aa..7e850f1 100644
--- a/src/robotics/kinematics.h
+++ b/src/robotics/kinematics.h
@@ -177,6 +177,10 @@ class Kinematics{
 
         }
 
+        void resetTravel(){
+            travel=0.0;
+        }
+
         float getX(){
             return m_to_mm(x);
         }