diff --git a/examples/firmware/firmware.ino b/examples/firmware/firmware.ino
index ce7b3f1..cb998a1 100644
--- a/examples/firmware/firmware.ino
+++ b/examples/firmware/firmware.ino
@@ -96,104 +96,113 @@ void loop(){
   }
   if (packeter.checkPayload()) {
     code = packeter.payloadTop();
-    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);
-        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;
+    if (!alvik.isBatteryAlert()){
+      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);
+          alvik.disableKinematicsMovement();
+          if (label=='L'){
+            switch (control_type){
+              case 'V':
+                alvik.disablePositionControlLeft();
+                alvik.setRpmLeft(value);
+                break;
+              case 'P':
+                alvik.setPositionLeft(value);
+                ack_required=MOVEMENT_LEFT;
+                ack_check=true;
+                break;
+              case 'Z':
+                alvik.resetPositionLeft(value);
+                break;
+            }
           }
-        }
-        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;
+          if (label=='R'){
+            switch (control_type){
+              case 'V':
+                alvik.disablePositionControlRight();
+                alvik.setRpmRight(value);
+                break;
+              case 'P':
+                alvik.setPositionRight(value);
+                ack_required=MOVEMENT_RIGHT;
+                ack_check=true;
+                break;
+              case 'Z':
+                alvik.resetPositionRight(value);
+                break;
+            }
           }
-        }
-        break;
-
-
-      case 'A':
-        packeter.unpacketC2F(code,position_left, position_right);
-        alvik.disableKinematicsMovement();
-        alvik.setPosition(position_left, position_right);
-        break;
-
-      
-      case 'S':
-        packeter.unpacketC2B(code,servo_A,servo_B);
-        alvik.setServoA(servo_A);
-        alvik.setServoB(servo_B);
-        break;
-      
-      case 'L':
-        packeter.unpacketC1B(code,leds);
-        alvik.setAllLeds(leds);
-        break;
-      
-      case 'P':
-        packeter.unpacketC1B3F(code,pid,kp,ki,kd);
-        if (pid=='L'){
-          alvik.setKPidLeft(kp,ki,kd);
-        }
-        if (pid=='R'){
-          alvik.setKPidRight(kp,ki,kd);
-        }
-        break;
-
-      case 'R':
-        packeter.unpacketC1F(code, value);
-        alvik.disablePositionControl();
-        alvik.rotate(value);
-        ack_required=MOVEMENT_ROTATE;
-        ack_check=true;
-        break;
-      
-      case 'G':
-        packeter.unpacketC1F(code, value);
-        alvik.disablePositionControl();
-        alvik.move(value);
-        ack_required=MOVEMENT_MOVE;
-        ack_check=true;
-        break;
-
-      case 'Z':
-        packeter.unpacketC3F(code, x, y, theta);
-        alvik.resetPose(x, y, theta);
-        break;
-
+          break;
+
+
+        case 'A':
+          packeter.unpacketC2F(code,position_left, position_right);
+          alvik.disableKinematicsMovement();
+          alvik.setPosition(position_left, position_right);
+          ack_required=MOVEMENT_POSITION;
+          ack_check=true;
+          break;
+
+        
+        case 'S':
+          packeter.unpacketC2B(code,servo_A,servo_B);
+          alvik.setServoA(servo_A);
+          alvik.setServoB(servo_B);
+          break;
+        
+        case 'L':
+          packeter.unpacketC1B(code,leds);
+          alvik.setAllLeds(leds);
+          break;
+        
+        case 'P':
+          packeter.unpacketC1B3F(code,pid,kp,ki,kd);
+          if (pid=='L'){
+            alvik.setKPidLeft(kp,ki,kd);
+          }
+          if (pid=='R'){
+            alvik.setKPidRight(kp,ki,kd);
+          }
+          break;
+
+        case 'R':
+          packeter.unpacketC1F(code, value);
+          alvik.disablePositionControl();
+          alvik.rotate(value);
+          ack_required=MOVEMENT_ROTATE;
+          ack_check=true;
+          break;
+        
+        case 'G':
+          packeter.unpacketC1F(code, value);
+          alvik.disablePositionControl();
+          alvik.move(value);
+          ack_required=MOVEMENT_MOVE;
+          ack_check=true;
+          break;
+
+        case 'Z':
+          packeter.unpacketC3F(code, x, y, theta);
+          alvik.resetPose(x, y, theta);
+          break;
+      }
+    }
+    switch (code){
       case 'X':
         packeter.unpacketC1B(code, ack_code);
         if (ack_code == 'K') {
@@ -207,6 +216,9 @@ void loop(){
           case 1: 
             alvik.setBehaviour(LIFT_ILLUMINATOR, true);
             break;
+          case 2:
+            alvik.setBehaviour(BATTERY_ALERT, true);
+            break;
           default:
             alvik.setBehaviour(ALL_BEHAVIOURS, false);
         }
@@ -227,6 +239,8 @@ void loop(){
         alvik.updateTouch();
         msg_size = packeter.packetC1B('t', alvik.getTouchKeys());
         alvik.serial->write(packeter.msg,msg_size);
+        msg_size = packeter.packetC1B('m', alvik.getMotion());
+        alvik.serial->write(packeter.msg,msg_size);
         break;
       case 2:
         alvik.updateAPDS();
@@ -278,13 +292,22 @@ void loop(){
       msg_size = packeter.packetC3B(0x7E, version[0], version[1], version[2]);
       alvik.serial->write(packeter.msg,msg_size);
     }
-    if (ack_check && alvik.isTargetReached()){
+    if (ack_check && (alvik.isTargetReached() || alvik.isPositionReached() || alvik.isPositionLeftReached() || alvik.isPositionRightReached())){
       if (ack_required == MOVEMENT_ROTATE){
         msg_size = packeter.packetC1B('x', 'R');
       }
       if (ack_required == MOVEMENT_MOVE){
         msg_size = packeter.packetC1B('x', 'M');
       }
+      if (ack_required == MOVEMENT_POSITION){
+        msg_size = packeter.packetC1B('x', 'P');
+      }
+      if (ack_required == MOVEMENT_LEFT){
+        msg_size = packeter.packetC1B('x', 'P');
+      }
+      if (ack_required == MOVEMENT_RIGHT){
+        msg_size = packeter.packetC1B('x', 'P');
+      }
     }
     else{
       msg_size = packeter.packetC1B('x', 0);
@@ -309,7 +332,7 @@ void loop(){
   if (millis()-tbattery>1000){
     tbattery = millis();
     alvik.updateBMS();
-    msg_size = packeter.packetC1F('p', alvik.getBatteryChargePercentage());
+    msg_size = packeter.packetC1F('p', alvik.isBatteryCharging()*alvik.getBatteryChargePercentage());
     alvik.serial->write(packeter.msg,msg_size);
   }
 }
diff --git a/library.properties b/library.properties
index 6b1cf07..a754aa2 100644
--- a/library.properties
+++ b/library.properties
@@ -1,5 +1,5 @@
 name=Arduino_AlvikCarrier
-version=1.0.1
+version=1.1.0
 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 a29c924..24a61c4 100644
--- a/src/Arduino_AlvikCarrier.cpp
+++ b/src/Arduino_AlvikCarrier.cpp
@@ -67,6 +67,20 @@ Arduino_AlvikCarrier::Arduino_AlvikCarrier(){
     ipKnobs = &iKnobs;
     imu_delta_time = MOTION_FX_ENGINE_DELTATIME;
     sample_to_discard = 0;
+    is_shaking = 0;
+    first_wakeup = true;
+    shake_time = 0;
+    shake_counter = 0;
+    tilt_status = 0x80;
+    xl = 0;
+    xh = 0;
+    yl = 0;
+    yh = 0;
+    zl = 0;
+    zh = 0;
+    tilt_time = 0;
+    tmp_tilt_status = 0;
+    tilt_filter = 0;
 
     // version
     version_high = VERSION_BYTE_HIGH;
@@ -123,7 +137,7 @@ int Arduino_AlvikCarrier::begin(){
      
     beginServo();
     
-    if (beginBMS()!=0){
+    if (!beginBMS()){
         errorLed(ERROR_BMS);
     }
     
@@ -255,14 +269,20 @@ void Arduino_AlvikCarrier::disconnectExternalI2C(){
 /******************************************************************************************************/
 
 int Arduino_AlvikCarrier::beginBMS(){
+    charging = 0.0;
     while(digitalRead(NANO_CHK)==HIGH){}
-    bms->begin();
-    return 0;
+    return bms->begin();
 }
 
 void Arduino_AlvikCarrier::updateBMS(){
     voltage = bms->readVCell();
     state_of_charge = bms->readSoc();
+    if (bms->readCurrent()>=0){
+        charging = 1.0;
+    }
+    else{
+        charging = -1.0;
+    }
 }
 
 
@@ -274,6 +294,14 @@ float Arduino_AlvikCarrier::getBatteryChargePercentage(){
     return state_of_charge;
 }
 
+float Arduino_AlvikCarrier::isBatteryCharging(){
+    return charging;
+}
+
+bool Arduino_AlvikCarrier::isBatteryAlert(){
+    return battery_alert;
+}
+
 
 
 /******************************************************************************************************/
@@ -348,6 +376,10 @@ float Arduino_AlvikCarrier::getPositionLeft(){
     return motor_control_left->getPosition();
 }
 
+bool Arduino_AlvikCarrier::isPositionLeftReached(){
+    return motor_control_left->isPositionReached();
+}
+
 void Arduino_AlvikCarrier::setPositionRight(const float degrees){
     motor_control_right->setPosition(degrees);
 }  
@@ -356,6 +388,10 @@ float Arduino_AlvikCarrier::getPositionRight(){
     return motor_control_right->getPosition();
 }
 
+bool Arduino_AlvikCarrier::isPositionRightReached(){
+    return motor_control_right->isPositionReached();
+}
+
 void Arduino_AlvikCarrier::setPosition(const float left_deg, const float right_deg){
     setPositionLeft(left_deg);
     setPositionRight(right_deg);
@@ -366,6 +402,10 @@ void Arduino_AlvikCarrier::getPosition(float & left_deg, float & right_deg){
     right_deg = getPositionRight();
 }
 
+bool Arduino_AlvikCarrier::isPositionReached(){
+    return isPositionLeftReached() && isPositionRightReached();
+}
+
 void Arduino_AlvikCarrier::resetPositionLeft(const float initial_position){
     motor_control_left->resetPosition(initial_position);
 }
@@ -619,6 +659,11 @@ int Arduino_AlvikCarrier::beginImu(){
     imu->Set_G_FS(2000);
     imu->Enable_X();
     imu->Enable_G();
+    imu->Enable_Wake_Up_Detection(LSM6DSO_INT1_PIN);
+    imu->Set_Wake_Up_Threshold(1);
+    imu->Set_Wake_Up_Duration(3);
+    imu->Enable_6D_Orientation(LSM6DSO_INT1_PIN);
+
 
     delay(10);
 
@@ -664,6 +709,60 @@ void Arduino_AlvikCarrier::updateImu(){
         sample_to_discard++;
     }
 
+    imu->Get_X_Event_Status(&imu_status);
+ 
+    if (imu_status.WakeUpStatus && 
+            (motor_control_left->getRPM()<1 && motor_control_left->getRPM()>-1) &&
+            (motor_control_right->getRPM()<1 && motor_control_right->getRPM()>-1)){
+        if (first_wakeup){
+            shake_time = millis();
+            first_wakeup = false;
+            shake_counter = 0;
+        }
+        shake_counter++;
+    }
+
+    if (millis()-shake_time>500){
+        if (shake_counter>10){
+            is_shaking = true;
+            shake_counter = 0;
+            shake_time_sig = millis();
+        }
+    }
+    if (is_shaking && (millis()-shake_time_sig>1000)){
+        is_shaking = false;
+        tilt_time = millis();
+    }
+
+    if ((!is_shaking) && (millis()-tilt_time>1000)){
+        imu->Get_6D_Orientation_XL(&xl);
+        imu->Get_6D_Orientation_XH(&xh);
+        imu->Get_6D_Orientation_YL(&yl);
+        imu->Get_6D_Orientation_YH(&yh);
+        imu->Get_6D_Orientation_ZL(&zl);
+        imu->Get_6D_Orientation_ZH(&zh);
+        
+        tmp_tilt_status = 0;
+        tmp_tilt_status |= xl<<4;
+        tmp_tilt_status |= xh<<5;
+        tmp_tilt_status |= zl<<7;
+        tmp_tilt_status |= zh<<6;
+        tmp_tilt_status |= yh<<3;
+        tmp_tilt_status |= yl<<2;
+
+        if (tilt_status !=  tmp_tilt_status){
+            tilt_filter++;
+        }else{
+            tilt_filter = 0;
+        }
+
+        if (tilt_filter>20){
+            tilt_status = tmp_tilt_status;
+            tilt_filter = 0;
+        }
+
+    }
+
 }
 
 float Arduino_AlvikCarrier::getAccelerationX(){
@@ -702,6 +801,14 @@ float Arduino_AlvikCarrier::getYaw(){
     return 360.0-filter_data.rotation[0];
 }
 
+bool Arduino_AlvikCarrier::isShaking(){
+    return is_shaking;
+}
+
+uint8_t Arduino_AlvikCarrier::getMotion(){
+    return tilt_status | isShaking();
+}
+
 
 
 /******************************************************************************************************/
@@ -877,43 +984,62 @@ void Arduino_AlvikCarrier::beginBehaviours(){
     prev_illuminator_state = illuminator_state;
     behaviours = 0;
     first_lift = true;
+    battery_alert_time = millis();
+    battery_alert_wave = 100;
+    battery_alert = false;
 }
 
 
 void Arduino_AlvikCarrier::updateBehaviours(){
-    if (behaviours|=1 == 1){
-        /*
+    
+    // illuminator off on lift
+    if ((1<<(LIFT_ILLUMINATOR-1)) & behaviours){
+
         if (isLifted()&&first_lift){
-            first_lift = false;
-            prev_illuminator_state = illuminator_state;
-            disableIlluminator();
+            //disableIlluminator();
+            setIlluminator(LOW);
+            first_lift=false;
         }
-        if (isLifted()&&!first_lift) {
-            if (prev_illuminator_state!=0){
-                disableIlluminator();
+        else{
+            if (!isLifted()){
+                setIlluminator(prev_illuminator_state);
             }
-        }
-        if (!isLifted()&&!first_lift){
-            if (prev_illuminator_state!=0){
-                //first_lift = true;
-                enableIlluminator();
+            if (!isLifted()&&!first_lift){
+                first_lift = true;
             }
         }
-        */
-       if (isLifted()&&first_lift){
-        //disableIlluminator();
-        setIlluminator(LOW);
-        first_lift=false;
-       }
-       else{
-        if (!isLifted()){
-            setIlluminator(prev_illuminator_state);
+    }
+
+    // battery alert
+    if ((1<<(BATTERY_ALERT-1)) & behaviours){
+        if ((isBatteryCharging()==-1)&&(getBatteryChargePercentage()<BATTERY_ALERT_MINIMUM_CHARGE)){
+            if (millis()-battery_alert_time>battery_alert_wave){
+                battery_alert_time = millis();
+                if (battery_alert_wave==400){
+                    setLeds(COLOR_RED);
+                    setLedBuiltin(HIGH);
+                    battery_alert_wave=100;
+                }
+                else{
+                    setLeds(COLOR_BLACK);
+                    setLedBuiltin(LOW);
+                    battery_alert_wave=400;
+                }
+            }
+            if (getBatteryChargePercentage()<BATTERY_ALERT_STOP_CHARGE){
+                battery_alert = true;
+                setRpm(0,0);
+                setIlluminator(false);
+            }
         }
-        if (!isLifted()&&!first_lift){
-            first_lift = true;
+        else{
+            battery_alert = false;
         }
-       }
     }
+    else{
+        battery_alert = false;
+    }
+
 }
 
 void Arduino_AlvikCarrier::setBehaviour(const uint8_t behaviour, const bool enable){
diff --git a/src/Arduino_AlvikCarrier.h b/src/Arduino_AlvikCarrier.h
index 3da1dbf..17c0165 100644
--- a/src/Arduino_AlvikCarrier.h
+++ b/src/Arduino_AlvikCarrier.h
@@ -45,7 +45,7 @@ class Arduino_AlvikCarrier{
 
 
         MAX17332 * bms;
-        float voltage, state_of_charge; 
+        float voltage, state_of_charge, charging; 
 
 
         AT42QT2120 * touch_sensor;
@@ -56,6 +56,8 @@ class Arduino_AlvikCarrier{
         LSM6DSOSensor * imu;
         int32_t accelerometer[3];
         int32_t gyroscope[3];
+        LSM6DSO_Event_Status_t imu_status;
+
 
         float imu_delta_time;
         MFX_knobs_t iKnobs;
@@ -64,6 +66,15 @@ class Arduino_AlvikCarrier{
         MFX_input_t imu_data;
         MFX_output_t filter_data;
         uint16_t sample_to_discard;
+        bool is_shaking;
+        bool first_wakeup;
+        unsigned long shake_time, shake_time_sig;
+        uint16_t shake_counter;
+        uint8_t tilt_status;
+        uint8_t xl, xh, yl, yh, zl, zh;
+        unsigned long tilt_time;
+        uint8_t tilt_filter;
+        uint8_t tmp_tilt_status;
 
         uint8_t version_high;
         uint8_t version_mid;
@@ -83,6 +94,9 @@ class Arduino_AlvikCarrier{
         bool prev_illuminator_state;
         uint8_t behaviours;
         bool first_lift;
+        unsigned long battery_alert_time;
+        unsigned long battery_alert_wave;
+        bool battery_alert;
 
 
 
@@ -145,6 +159,8 @@ class Arduino_AlvikCarrier{
         void updateBMS();                                                               // update the BMS
         float getBatteryVoltage();                                                      // get Voltage
         float getBatteryChargePercentage();                                             // get battery percentage
+        float isBatteryCharging();                                                      // return -1 if battery discharge or 1 if charging
+        bool isBatteryAlert();                                                          // return true if battery is LOW and alert behaviour is setted, otherwise false
 
 
 
@@ -161,10 +177,13 @@ class Arduino_AlvikCarrier{
         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
+        bool isPositionLeftReached();                                                   // return true if left wheel position is reached
         void setPositionRight(const float degrees);                                     // set position in degrees on right wheel
         float getPositionRight();                                                       // get right wheel position in degrees
+        bool isPositionRightReached();                                                  // return true if right wheel position is reached
         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
+        bool isPositionReached();                                                       // return true if both wheels position are reached
         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
@@ -173,7 +192,6 @@ class Arduino_AlvikCarrier{
 
 
 
-
         // Touch
         int beginTouch();                                                               // initialize touch
         void updateTouch();                                                             // update touch status
@@ -220,6 +238,8 @@ class Arduino_AlvikCarrier{
         float getRoll();                                                                // get robot roll
         float getPitch();                                                               // get robot pitch
         float getYaw();                                                                 // get robot yaw
+        bool  isShaking();                                                              // get true if robot is shaking    
+        uint8_t getMotion();                                                            // get tilt + shaking                                                    
 
         void errorLed(const int error_code);                                            // error routine, locks on code blinking led
 
diff --git a/src/definitions/robot_definitions.h b/src/definitions/robot_definitions.h
index 0562bf5..bae10e0 100644
--- a/src/definitions/robot_definitions.h
+++ b/src/definitions/robot_definitions.h
@@ -37,6 +37,7 @@ const float MOTOR_RATIO = MOTOR_CPR*MOTOR_GEAR_RATIO;
 #define POSITION_KD_DEFAULT 0.0001
 #define POSITION_CONTROL_PERIOD 0.02
 #define POSITION_MAX_SPEED 30.0
+#define POSITION_THRESHOLD 10
 
 
 
@@ -58,12 +59,16 @@ const float MOTOR_RATIO = MOTOR_CPR*MOTOR_GEAR_RATIO;
 #define MOVEMENT_DISABLED 0
 #define MOVEMENT_ROTATE 1
 #define MOVEMENT_MOVE 2
+#define MOVEMENT_LEFT 3
+#define MOVEMENT_RIGHT 4
+#define MOVEMENT_POSITION 5
 
 
 #define LIFT_THRESHOLD 80
 
 #define ALL_BEHAVIOURS 255
 #define LIFT_ILLUMINATOR 1
+#define BATTERY_ALERT 2
 
 
 
@@ -87,8 +92,12 @@ const float MOTION_FX_PERIOD = (1000U / MOTION_FX_FREQ);
 
 // Library version
 #define VERSION_BYTE_HIGH 1
-#define VERSION_BYTE_MID 0
-#define VERSION_BYTE_LOW 1
+#define VERSION_BYTE_MID 1
+#define VERSION_BYTE_LOW 0
+
+// Battery stats
+#define BATTERY_ALERT_MINIMUM_CHARGE 20.0
+#define BATTERY_ALERT_STOP_CHARGE 10.0
 
 
 
diff --git a/src/motor_control/motor_control.cpp b/src/motor_control/motor_control.cpp
index 13aa831..843061f 100644
--- a/src/motor_control/motor_control.cpp
+++ b/src/motor_control/motor_control.cpp
@@ -35,6 +35,7 @@ MotorControl::MotorControl(DCmotor * _motor, Encoder * _encoder, const float _kp
     pos_controller_period = _pos_controller_period;
     pos_max_velocity = _pos_max_velocity;
     position_control_enabled = false;
+    is_position_reached = false;
 
 
 
@@ -168,6 +169,9 @@ void MotorControl::update(){
     if (position_control_enabled){
         pos_pid->update(position);
         setRPM(round(pos_pid->getControlOutput()/10.0)*10);
+        if (abs(pos_pid->getError())<POSITION_THRESHOLD){
+            is_position_reached = true;
+        }
     }
 }
 
@@ -210,6 +214,7 @@ void MotorControl::enablePositionControl(){
 
 void MotorControl::disablePositionControl(){
     position_control_enabled = false;
+    is_position_reached = false;
 }
 
 bool MotorControl::isPositionControlEnabled(){
@@ -230,5 +235,10 @@ float MotorControl::getPosition(){
 void MotorControl::setPosition(const float degree){
     pos_pid->setReference(degree);
     enablePositionControl();
+    is_position_reached = false;
+}
+
+bool MotorControl::isPositionReached(){
+    return is_position_reached;
 }
 
diff --git a/src/motor_control/motor_control.h b/src/motor_control/motor_control.h
index e3ffa5a..3adffb4 100644
--- a/src/motor_control/motor_control.h
+++ b/src/motor_control/motor_control.h
@@ -43,6 +43,7 @@ class MotorControl{
         float pos_controller_period;
         float pos_max_velocity;
         bool position_control_enabled;
+        bool is_position_reached;
         
         float position;
         float angle;
@@ -112,6 +113,7 @@ class MotorControl{
         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
+        bool isPositionReached();
 
         float getError();