diff --git a/include/arm_control.h b/include/arm_control.h
index e1ca3e2eb9f00434fae31b581d532663528bbbde..4971fb5d0ebfcfdf03fbfcabc3a1c4360a2f4c97 100644
--- a/include/arm_control.h
+++ b/include/arm_control.h
@@ -63,6 +63,8 @@ class CArmControl
     imu_control_mode_t imu_control_mode;
     double imu_threshold;
     double imu_gain;
+    // servo power 
+    bool power_enabled;
   protected:
     static void *position_monitor_thread(void *param);
     void move_joints(void);
@@ -83,6 +85,9 @@ class CArmControl
     void move(unsigned int axis_id,double torque);
     void stop(unsigned int axis_id=ALL);
     void get_current_angles(double &pan, double &tilt,double &roll);
+    void enable_servo_power(void);
+    void disable_servo_power(void);
+    bool is_servo_power_enabled(void);
     // stored poses
     void increase_max_speed(void);
     void decrease_max_speed(void);
diff --git a/src/CMakeLists.txt b/src/CMakeLists.txt
index 312efb4b3f481bb298eee8aaa141a544de6725cd..5bd7bee44dab5f5bbfb9afc530c5a4ebedd12d8c 100644
--- a/src/CMakeLists.txt
+++ b/src/CMakeLists.txt
@@ -26,10 +26,12 @@ TARGET_LINK_LIBRARIES(arm_control ${comm_LIBRARY})
 TARGET_LINK_LIBRARIES(arm_control ${dynamixel_LIBRARY})
 TARGET_LINK_LIBRARIES(arm_control ${dynamixel_motor_cont_LIBRARY})
 TARGET_LINK_LIBRARIES(arm_control ${XSD_LIBRARY})
-#TARGET_LINK_LIBRARIES(arm_control libwiringPi.so)
+TARGET_LINK_LIBRARIES(arm_control libwiringPi.so)
 
 ADD_DEPENDENCIES(arm_control xsd_files_gen)
 
+ADD_DEFINITIONS(-DPI)
+
 # link necessary libraries
 INSTALL(TARGETS arm_control
         RUNTIME DESTINATION bin
diff --git a/src/arm_control.cpp b/src/arm_control.cpp
index a8c0e24d8642bcd619ff5ee37ec51f7183353e42..0d45ab978de660290e967e637aeb3557f9b405c2 100644
--- a/src/arm_control.cpp
+++ b/src/arm_control.cpp
@@ -17,8 +17,9 @@
 #include "xml/arm_motions_cfg_file.hxx"
 
 #define OK_LED  25
-#define IMU_MODE 23
-#define IMU_CAL 21
+#define POWERED 23
+
+#define MOTION_PERIOD    0.01
 
 #define MAX_PAN_ANGLE    80.0
 #define MIN_PAN_ANGLE    -80.0
@@ -43,13 +44,12 @@ CArmControl::CArmControl(std::string &dyn_serial, unsigned int baudrate,unsigned
 
 #ifdef PI
   wiringPiSetup();
-  pinMode(IMU_CAL,OUTPUT);
-  digitalWrite(IMU_CAL,LOW);
-  pinMode(IMU_MODE,OUTPUT);
-  digitalWrite(IMU_MODE,LOW);
+  pinMode(POWERED,OUTPUT);
+  digitalWrite(POWERED,LOW);
   pinMode(OK_LED,OUTPUT);
   digitalWrite(OK_LED,LOW);
 #endif
+  this->power_enabled=true;
   this->exit_flag=false; 
   // detect and initialize servos
   this->dyn_server=CDynamixelServerSerial::instance();
@@ -87,7 +87,7 @@ CArmControl::CArmControl(std::string &dyn_serial, unsigned int baudrate,unsigned
   this->enabled.roll=false;
   this->stored_poses.resize(4);
   this->max_speed=MAX_SPEED;
-  this->max_accel=100.0;
+  this->max_accel=30.0;
   this->target_angles.pan=this->motors[PAN]->get_current_angle();
   this->target_speeds.pan=this->max_speed;
   this->target_accels.pan=this->max_accel;
@@ -267,30 +267,33 @@ void *CArmControl::position_monitor_thread(void *param)
 
 void CArmControl::move_joints(void)
 {
+  static double period=0;
   bool moving=false;
   std::string name;
 
-  if(this->target_run)
+  period-=0.01;
+  if(this->target_run && period<=0.0) 
   {
+    period=MOTION_PERIOD;
     if(abs(this->target_angles.pan-this->current_angles.pan)>=1.0)
     {
       moving=true;
       if(this->current_speeds.pan!=(this->target_dir.pan*this->target_speeds.pan))// it is necessary to change the current speed
       {
-        this->current_angles.pan+=this->current_speeds.pan*0.01/2.0;
+        this->current_angles.pan+=this->current_speeds.pan*MOTION_PERIOD/2.0;
         if(this->current_speeds.pan>this->target_dir.pan*this->target_speeds.pan)
         {
-          this->current_speeds.pan-=this->target_accels.pan*0.01;// reduce speed
+          this->current_speeds.pan-=this->target_accels.pan*MOTION_PERIOD;// reduce speed
           if(this->current_speeds.pan<this->target_dir.pan*this->target_speeds.pan)
             this->current_speeds.pan=this->target_dir.pan*this->target_speeds.pan;
         }
         else
         {
-          this->current_speeds.pan+=this->target_accels.pan*0.01;// increase speed
+          this->current_speeds.pan+=this->target_accels.pan*MOTION_PERIOD;// increase speed
           if(this->current_speeds.pan>this->target_dir.pan*this->target_speeds.pan)
             this->current_speeds.pan=this->target_dir.pan*this->target_speeds.pan;
         }
-        this->current_angles.pan+=this->current_speeds.pan*0.01/2.0;
+        this->current_angles.pan+=this->current_speeds.pan*MOTION_PERIOD/2.0;
         if(this->target_dir.pan==1.0 && this->current_angles.pan>this->target_angles.pan)
           this->current_angles.pan=this->target_angles.pan;
         if(this->target_dir.pan==-1.0 && this->current_angles.pan<this->target_angles.pan)
@@ -300,7 +303,7 @@ void CArmControl::move_joints(void)
       {
         if(abs(this->target_angles.pan-this->current_angles.pan)>this->target_stop_angles.pan)// constant speed phase
         {
-          this->current_angles.pan+=this->current_speeds.pan*0.01;
+          this->current_angles.pan+=this->current_speeds.pan*MOTION_PERIOD;
           if(this->target_dir.pan==1.0 && this->current_angles.pan>this->target_angles.pan)
             this->current_angles.pan=this->target_angles.pan;
           if(this->target_dir.pan==-1.0 && this->current_angles.pan<this->target_angles.pan)
@@ -308,12 +311,12 @@ void CArmControl::move_joints(void)
         }
         else// deceleration phase
         {
-          this->current_angles.pan=this->current_angles.pan+this->current_speeds.pan*0.01/2.0;
-          this->target_speeds.pan=this->target_speeds.pan-this->target_accels.pan*0.01;
+          this->current_angles.pan=this->current_angles.pan+this->current_speeds.pan*MOTION_PERIOD/2.0;
+          this->target_speeds.pan=this->target_speeds.pan-this->target_accels.pan*MOTION_PERIOD;
           if(this->target_speeds.pan<0)
             this->target_speeds.pan=0;
           this->current_speeds.pan=this->target_dir.pan*this->target_speeds.pan;
-          this->current_angles.pan=this->current_angles.pan+this->current_speeds.pan*0.01/2.0;
+          this->current_angles.pan=this->current_angles.pan+this->current_speeds.pan*MOTION_PERIOD/2.0;
           if(this->target_dir.pan==1.0 && this->current_angles.pan>this->target_angles.pan)
             this->current_angles.pan=this->target_angles.pan;
           if(this->target_dir.pan==-1.0 && this->current_angles.pan<this->target_angles.pan)
@@ -328,20 +331,20 @@ void CArmControl::move_joints(void)
       moving=true;
       if(this->current_speeds.tilt!=(this->target_dir.tilt*this->target_speeds.tilt))// it is necessary to change the current speed
       {
-        this->current_angles.tilt+=this->current_speeds.tilt*0.01/2.0;
+        this->current_angles.tilt+=this->current_speeds.tilt*MOTION_PERIOD/2.0;
         if(this->current_speeds.tilt>this->target_dir.tilt*this->target_speeds.tilt)
         {
-          this->current_speeds.tilt-=this->target_accels.tilt*0.01;// reduce speed
+          this->current_speeds.tilt-=this->target_accels.tilt*MOTION_PERIOD;// reduce speed
           if(this->current_speeds.tilt<this->target_dir.tilt*this->target_speeds.tilt)
             this->current_speeds.tilt=this->target_dir.tilt*this->target_speeds.tilt;
         }
         else
         {
-          this->current_speeds.tilt+=this->target_accels.tilt*0.01;// increase speed
+          this->current_speeds.tilt+=this->target_accels.tilt*MOTION_PERIOD;// increase speed
           if(this->current_speeds.tilt>this->target_dir.tilt*this->target_speeds.tilt)
             this->current_speeds.tilt=this->target_dir.tilt*this->target_speeds.tilt;
         }
-        this->current_angles.tilt+=this->current_speeds.tilt*0.01/2.0;
+        this->current_angles.tilt+=this->current_speeds.tilt*MOTION_PERIOD/2.0;
         if(this->target_dir.tilt==1.0 && this->current_angles.tilt>this->target_angles.tilt)
           this->current_angles.tilt=this->target_angles.tilt;
         if(this->target_dir.tilt==-1.0 && this->current_angles.tilt<this->target_angles.tilt)
@@ -351,7 +354,7 @@ void CArmControl::move_joints(void)
       {
         if(abs(this->target_angles.tilt-this->current_angles.tilt)>this->target_stop_angles.tilt)// constant speed phase
         {
-          this->current_angles.tilt+=this->current_speeds.tilt*0.01;
+          this->current_angles.tilt+=this->current_speeds.tilt*MOTION_PERIOD;
           if(this->target_dir.tilt==1.0 && this->current_angles.tilt>this->target_angles.tilt)
             this->current_angles.tilt=this->target_angles.tilt;
           if(this->target_dir.tilt==-1.0 && this->current_angles.tilt<this->target_angles.tilt)
@@ -359,12 +362,12 @@ void CArmControl::move_joints(void)
         }
         else// deceleration phase
         {
-          this->current_angles.tilt=this->current_angles.tilt+this->current_speeds.tilt*0.01/2.0;
-          this->target_speeds.tilt=this->target_speeds.tilt-this->target_accels.tilt*0.01;
+          this->current_angles.tilt=this->current_angles.tilt+this->current_speeds.tilt*MOTION_PERIOD/2.0;
+          this->target_speeds.tilt=this->target_speeds.tilt-this->target_accels.tilt*MOTION_PERIOD;
           if(this->target_speeds.tilt<0)
             this->target_speeds.tilt=0;
           this->current_speeds.tilt=this->target_dir.tilt*this->target_speeds.tilt;
-          this->current_angles.tilt=this->current_angles.tilt+this->current_speeds.tilt*0.01/2.0;
+          this->current_angles.tilt=this->current_angles.tilt+this->current_speeds.tilt*MOTION_PERIOD/2.0;
           if(this->target_dir.tilt==1.0 && this->current_angles.tilt>this->target_angles.tilt)
             this->current_angles.tilt=this->target_angles.tilt;
           if(this->target_dir.tilt==-1.0 && this->current_angles.tilt<this->target_angles.tilt)
@@ -379,20 +382,20 @@ void CArmControl::move_joints(void)
       moving=true;
       if(this->current_speeds.roll!=(this->target_dir.roll*this->target_speeds.roll))// it is necessary to change the current speed
       {
-        this->current_angles.roll+=this->current_speeds.roll*0.01/2.0;
+        this->current_angles.roll+=this->current_speeds.roll*MOTION_PERIOD/2.0;
         if(this->current_speeds.roll>this->target_dir.roll*this->target_speeds.roll)
         {
-          this->current_speeds.roll-=this->target_accels.roll*0.01;// reduce speed
+          this->current_speeds.roll-=this->target_accels.roll*MOTION_PERIOD;// reduce speed
           if(this->current_speeds.roll<this->target_dir.roll*this->target_speeds.roll)
             this->current_speeds.roll=this->target_dir.roll*this->target_speeds.roll;
         }
         else
         {
-          this->current_speeds.roll+=this->target_accels.roll*0.01;// increase speed
+          this->current_speeds.roll+=this->target_accels.roll*MOTION_PERIOD;// increase speed
           if(this->current_speeds.roll>this->target_dir.roll*this->target_speeds.roll)
             this->current_speeds.roll=this->target_dir.roll*this->target_speeds.roll;
         }
-        this->current_angles.roll+=this->current_speeds.roll*0.01/2.0;
+        this->current_angles.roll+=this->current_speeds.roll*MOTION_PERIOD/2.0;
         if(this->target_dir.roll==1.0 && this->current_angles.roll>this->target_angles.roll)
           this->current_angles.roll=this->target_angles.roll;
         if(this->target_dir.roll==-1.0 && this->current_angles.roll<this->target_angles.roll)
@@ -402,7 +405,7 @@ void CArmControl::move_joints(void)
       {
         if(abs(this->target_angles.roll-this->current_angles.roll)>this->target_stop_angles.roll)// constant speed phase
         {
-          this->current_angles.roll+=this->current_speeds.roll*0.01;
+          this->current_angles.roll+=this->current_speeds.roll*MOTION_PERIOD;
           if(this->target_dir.roll==1.0 && this->current_angles.roll>this->target_angles.roll)
             this->current_angles.roll=this->target_angles.roll;
           if(this->target_dir.roll==-1.0 && this->current_angles.roll<this->target_angles.roll)
@@ -410,12 +413,12 @@ void CArmControl::move_joints(void)
         }
         else// deceleration phase
         {
-          this->current_angles.roll=this->current_angles.roll+this->current_speeds.roll*0.01/2.0;
-          this->target_speeds.roll=this->target_speeds.roll-this->target_accels.roll*0.01;
+          this->current_angles.roll=this->current_angles.roll+this->current_speeds.roll*MOTION_PERIOD/2.0;
+          this->target_speeds.roll=this->target_speeds.roll-this->target_accels.roll*MOTION_PERIOD;
           if(this->target_speeds.roll<0)
             this->target_speeds.roll=0;
           this->current_speeds.roll=this->target_dir.roll*this->target_speeds.roll;
-          this->current_angles.roll=this->current_angles.roll+this->current_speeds.roll*0.01/2.0;
+          this->current_angles.roll=this->current_angles.roll+this->current_speeds.roll*MOTION_PERIOD/2.0;
           if(this->target_dir.roll==1.0 && this->current_angles.roll>this->target_angles.roll)
             this->current_angles.roll=this->target_angles.roll;
           if(this->target_dir.roll==-1.0 && this->current_angles.roll<this->target_angles.roll)
@@ -786,6 +789,27 @@ void CArmControl::set_max_speed(double speed)
   this->access.exit();
 }
 
+void CArmControl::enable_servo_power(void)
+{
+  #ifdef PI
+    digitalWrite(POWERED,LOW);
+  #endif
+  this->power_enabled=true;
+}
+
+void CArmControl::disable_servo_power(void)
+{
+  #ifdef PI
+    digitalWrite(POWERED,HIGH);
+  #endif
+  this->power_enabled=false;
+}
+
+bool CArmControl::is_servo_power_enabled(void)
+{
+  return this->power_enabled;
+}
+
 void CArmControl::load_poses(const std::string &filename)
 {
   arm_motions_t::arm_motion_iterator iterator;
@@ -1046,9 +1070,6 @@ void CArmControl::enable_imu(double pan,double tilt, double roll)
 {
   this->access.enter();
   this->imu_enabled=true;
-  #ifdef PI
-  digitalWrite(IMU_MODE,HIGH);
-  #endif 
   this->imu_zero.pan=pan;
   this->imu_last_angles.pan=this->current_angles.pan;
   this->imu_zero.tilt=tilt;
@@ -1062,9 +1083,6 @@ void CArmControl::disable_imu(void)
 {
   this->access.enter();
   this->imu_enabled=false;
-  #ifdef PI
-  digitalWrite(IMU_MODE,LOW);
-  #endif
   this->access.exit();
   this->stop();
 }
@@ -1155,8 +1173,7 @@ CArmControl::~CArmControl()
       this->motors[ROLL]=NULL;
     }
   #ifdef PI
-  digitalWrite(IMU_CAL,LOW);
-  digitalWrite(IMU_MODE,LOW);
+  digitalWrite(POWERED,LOW);
   digitalWrite(OK_LED,LOW);
   #endif
 }