diff --git a/src/CMakeLists.txt b/src/CMakeLists.txt
index 322129d27c0c8fae87e8241da77b532b1a595cad..dac5dab2de629ddc397fd4b9eaa67b9fcf4a101c 100644
--- a/src/CMakeLists.txt
+++ b/src/CMakeLists.txt
@@ -1,5 +1,3 @@
-SET(DARWIN_FW_PATH ~/humanoids/darwin_stm32_fw)
-
 ADD_SUBDIRECTORY(xml)
 IF(HAVE_XSD)
   ADD_DEFINITIONS(-D_HAVE_XSD)
@@ -38,7 +36,6 @@ ENDIF(EIGEN3_FOUND)
 
 # add the necessary include directories
 INCLUDE_DIRECTORIES(.)
-INCLUDE_DIRECTORIES(${DARWIN_FW_PATH}/include)
 INCLUDE_DIRECTORIES(${iriutils_INCLUDE_DIR})
 INCLUDE_DIRECTORIES(${dynamixel_INCLUDE_DIR})
 INCLUDE_DIRECTORIES(${EIGEN3_INCLUDE_DIR})
diff --git a/src/darwin_robot.cpp b/src/darwin_robot.cpp
index 600d0834d28c8ef8eb7e9fb3485414901299d3dc..1304851887bd3fbf35314a49cf2a92f2d6453b6b 100644
--- a/src/darwin_robot.cpp
+++ b/src/darwin_robot.cpp
@@ -46,6 +46,7 @@ const std::string servo_names[MAX_NUM_SERVOS]={std::string("Servo0"),
                                                std::string("Servo29"),
                                                std::string("Servo30")};
 
+/*
 const led_info_t leds[GPIO_NUM_LEDS]={{DARWIN_TX_LED_CNTRL,DARWIN_TX_LED_PERIOD_L,GPIO_VALUE,GPIO_TOGGLE,GPIO_BLINK},
                                       {DARWIN_RX_LED_CNTRL,DARWIN_RX_LED_PERIOD_L,GPIO_VALUE,GPIO_TOGGLE,GPIO_BLINK},
                                       {DARWIN_PLAY_LED_CNTRL,DARWIN_PLAY_LED_PERIOD_L,GPIO_VALUE,GPIO_TOGGLE,GPIO_BLINK},
@@ -57,7 +58,7 @@ const led_info_t leds[GPIO_NUM_LEDS]={{DARWIN_TX_LED_CNTRL,DARWIN_TX_LED_PERIOD_
                                       {DARWIN_AUX2_LED_CNTRL,DARWIN_AUX2_LED_COLOR_L,GPIO_VALUE_R,GPIO_TOGGLE_R,0x00},
                                       {DARWIN_AUX2_LED_CNTRL,DARWIN_AUX2_LED_COLOR_L,GPIO_VALUE_G,GPIO_TOGGLE_G,0x00},
                                       {DARWIN_AUX2_LED_CNTRL,DARWIN_AUX2_LED_COLOR_L,GPIO_VALUE_B,GPIO_TOGGLE_B,0x00}};
-
+*/
 
 CDarwinRobot::CDarwinRobot(const std::string &name,std::string &bus_id,int bus_speed, unsigned char id,bool sim)
 {
@@ -86,20 +87,21 @@ CDarwinRobot::CDarwinRobot(const std::string &name,std::string &bus_id,int bus_s
       try{
         this->robot_device=dyn_server->get_device(id,dyn_version2);
         this->robot_id=id;
+        this->manager_period=this->manager_get_period();
         /* wait for the firmware to finish the scan */
-        do{
+//        do{
           /* get the current manager status */
-          this->robot_device->read_byte_register(DARWIN_MM_CNTRL,&this->manager_status);
-          usleep(10000);
-        }while(this->manager_status&MANAGER_SCANNING);
+//          this->robot_device->read_byte_register(DARWIN_MM_CNTRL,&this->manager_status);
+//          usleep(10000);
+//        }while(this->manager_status&MANAGER_SCANNING);
         /* get the number of servos detected */
-        this->robot_device->read_byte_register(DARWIN_MM_NUM_SERVOS,&this->manager_num_servos);
+//        this->robot_device->read_byte_register(DARWIN_MM_NUM_SERVOS,&this->manager_num_servos);
         /* get the current action status */
-        this->robot_device->read_byte_register(DARWIN_ACTION_CNTRL,&this->action_status);
+//        this->robot_device->read_byte_register(DARWIN_ACTION_CNTRL,&this->action_status);
         /* get the current smart charger status (detected or not)*/
-        this->robot_device->read_byte_register(DARWIN_SMART_CHARGER_CNTRL,&this->smart_charger_status);
-        this->min_limit_current=MIN_LIMIT_CURRENT;
-        this->max_limit_current=MAX_LIMIT_CURRENT;
+//        this->robot_device->read_byte_register(DARWIN_SMART_CHARGER_CNTRL,&this->smart_charger_status);
+//        this->min_limit_current=MIN_LIMIT_CURRENT;
+//        this->max_limit_current=MAX_LIMIT_CURRENT;
       }catch(CDynamixelServerException &e){
         this->dyn_server->free_device(id);
         std::cout << "exception" << std::endl;
@@ -120,6 +122,7 @@ CDarwinRobot::CDarwinRobot(const std::string &name,std::string &bus_id,int bus_s
 }
 
 // GPIO interface
+/*
 void CDarwinRobot::gpio_set_led(led_t led)
 {
   if(this->robot_device!=NULL)
@@ -172,7 +175,7 @@ void CDarwinRobot::gpio_aux1_color(double red, double green, double blue)
   if(this->robot_device!=NULL)
   {
     data[0]=GPIO_COLOR;
-    /* saturate the color */
+    // saturate the color
     if(red>1.0) red=1.0;
     else if(red<0.0) red=0.0;
     if(green>1.0) green=1.0;
@@ -196,7 +199,7 @@ void CDarwinRobot::gpio_aux2_color(double red, double green, double blue)
   if(this->robot_device!=NULL)
   {
     data[0]=GPIO_COLOR;
-    /* saturate the color */
+    // saturate the color
     if(red>1.0) red=1.0;
     else if(red<0.0) red=0.0;
     if(green>1.0) green=1.0;
@@ -233,8 +236,9 @@ bool CDarwinRobot::is_button_pressed(pushbutton_t pushbutton)
   else
     throw CDarwinRobotException(_HERE_,"Invalid robot device");
 }
-
+*/
 // ADC interface
+/*
 void CDarwinRobot::adc_start(void)
 {
   if(this->robot_device!=NULL)
@@ -301,8 +305,9 @@ void CDarwinRobot::adc_stop(void)
   else
     throw CDarwinRobotException(_HERE_,"Invalid robot device");
 }
-
+*/
 // imu interface
+/*
 void CDarwinRobot::imu_start(void)
 {
   if(this->robot_device!=NULL)
@@ -427,91 +432,279 @@ void CDarwinRobot::imu_get_gyro_data(double *x,double *y,double *z)
   else
     throw CDarwinRobotException(_HERE_,"Invalid robot device");
 }
-
+*/
 // motion manager interface
-void CDarwinRobot::mm_set_period(double period_ms)
+double CDarwinRobot::manager_get_period(void)
 {
-  unsigned short period;
+  unsigned char period;
 
-  if(this->robot_device!=NULL)
-  {
-    period=(period_ms*1000.0);
-    this->robot_device->write_word_register(DARWIN_MM_PERIOD_L,period);
-  }
-  else
+  if(this->robot_device==NULL)
+    throw CDarwinRobotException(_HERE_,"Invalid robot device");
+  this->robot_device->read_byte_register(MANAGER_PERIOD,&period);
+  return ((double)period/1000.0);
+}
+
+void CDarwinRobot::manager_set_period(double period_s)
+{
+  unsigned char period;
+
+  if(this->robot_device==NULL)
+    throw CDarwinRobotException(_HERE_,"Invalid robot device");
+  period=(period_s*1000.0);
+  this->robot_device->write_byte_register(MANAGER_PERIOD,period);
+  this->manager_period=period_s;
+}
+
+unsigned int CDarwinRobot::manager_get_num_modules(void)
+{
+  unsigned char num;
+
+  if(this->robot_device==NULL)
+    throw CDarwinRobotException(_HERE_,"Invalid robot device");
+  this->robot_device->read_byte_register(MANAGER_NUM_MODULES,&num);
+  return num;
+}
+
+unsigned int CDarwinRobot::managet_get_num_masters(void)
+{
+  unsigned char num;
+
+  if(this->robot_device==NULL)
+    throw CDarwinRobotException(_HERE_,"Invalid robot device");
+  this->robot_device->read_byte_register(MANAGER_PERIOD,&num);
+  return num;
+}
+
+void CDarwinRobot::manager_start(void)
+{
+  if(this->robot_device==NULL)
+    throw CDarwinRobotException(_HERE_,"Invalid robot device");
+  this->robot_device->write_byte_register(MANAGER_CONTROL,MANAGER_START);
+}
+
+void CDarwinRobot::manager_stop(void)
+{
+  if(this->robot_device==NULL)
+    throw CDarwinRobotException(_HERE_,"Invalid robot device");
+  this->robot_device->write_byte_register(MANAGER_CONTROL,MANAGER_STOP);
+}
+
+bool CDarwinRobot::manager_is_running(void)
+{
+  unsigned char running;
+  
+  if(this->robot_device==NULL)
+    throw CDarwinRobotException(_HERE_,"Invalid robot device");
+  this->robot_device->read_byte_register(MANAGER_CONTROL,&running);
+  return running&MANAGER_RUNNING;
+}
+
+void CDarwinRobot::manager_start_scan(void)
+{
+  if(this->robot_device==NULL)
     throw CDarwinRobotException(_HERE_,"Invalid robot device");
+  this->robot_device->write_byte_register(MANAGER_CONTROL,MANAGER_START_SCAN);
+}
+
+bool CDarwinRobot::manager_is_scanning(void)
+{
+  unsigned char scanning;
+  
+  if(this->robot_device==NULL)
+    throw CDarwinRobotException(_HERE_,"Invalid robot device");
+  this->robot_device->read_byte_register(MANAGER_CONTROL,&scanning);
+  return scanning&MANAGER_SCANNING;
+}
+
+unsigned char CDarwinRobot::manager_get_num_devices(void)
+{
+  unsigned char num;
+
+  if(this->robot_device==NULL)
+    throw CDarwinRobotException(_HERE_,"Invalid robot device");
+  this->robot_device->read_byte_register(MANAGER_NUM_DEVICES,&num);
+  return num;
+}
+
+/* motion manager interface */
+void CDarwinRobot::mm_set_period(double period_s)
+{
+  unsigned char period;
+
+  if(this->robot_device==NULL)
+    throw CDarwinRobotException(_HERE_,"Invalid robot device");
+  period=(period_s/this->manager_period);
+  this->robot_device->write_word_register(MMANAGER_PERIOD,period);
 }
 
 double CDarwinRobot::mm_get_period(void)
 {
   unsigned short period;
 
+  if(this->robot_device==NULL)
+    throw CDarwinRobotException(_HERE_,"Invalid robot device");
+  this->robot_device->read_word_register(MMANAGER_PERIOD,&period);
+  return ((double)period*this->manager_period);
+}
+
+void CDarwinRobot::mm_enable_servo(unsigned char servo_id)
+{
+  unsigned char value;
+
   if(this->robot_device!=NULL)
   {
-    this->robot_device->read_word_register(DARWIN_MM_PERIOD_L,&period);
-    return ((double)period)/1000.0;
+    if(servo_id>MAX_NUM_SERVOS)
+    {
+      throw CDarwinRobotException(_HERE_,"Invalid servo identifier");
+    }
+    // get the current value
+    this->robot_device->read_byte_register(MMANAGER_ENABLE+servo_id/2,&value);
+    if(servo_id%2)// odd servo
+      value|=MMANAGER_ODD_SER_EN;
+    else// even servo
+      value|=MMANAGER_EVEN_SER_EN;
+    this->robot_device->write_byte_register(MMANAGER_ENABLE+servo_id/2,value);
   }
   else
     throw CDarwinRobotException(_HERE_,"Invalid robot device");
 }
 
-unsigned char CDarwinRobot::mm_get_num_servos(void)
+void CDarwinRobot::mm_disable_servo(unsigned char servo_id)
 {
+  unsigned char value;
+
   if(this->robot_device!=NULL)
   {
-    this->robot_device->read_byte_register(DARWIN_MM_NUM_SERVOS,&this->manager_num_servos);
-    return this->manager_num_servos;
+    if(servo_id>MAX_NUM_SERVOS)
+    {
+      throw CDarwinRobotException(_HERE_,"Invalid servo identifier");
+    }
+    // get the current value
+    this->robot_device->read_byte_register(MMANAGER_ENABLE+servo_id/2,&value);
+    if(servo_id%2)// odd servo
+      value&=(~MMANAGER_ODD_SER_EN);
+    else// even servo
+      value&=(~MMANAGER_EVEN_SER_EN);
+    this->robot_device->write_byte_register(MMANAGER_ENABLE+servo_id/2,value);
   }
   else
     throw CDarwinRobotException(_HERE_,"Invalid robot device");
 }
 
-unsigned int CDarwinRobot::mm_get_present_servos(void)
+bool CDarwinRobot::mm_is_servo_enabled(unsigned char servo_id)
 {
-  unsigned int present_servos;
+  unsigned char value;
 
   if(this->robot_device!=NULL)
   {
-    this->robot_device->read_registers(DARWIN_MM_PRESENT_SERVOS1,(unsigned char *)&present_servos,4);
-    return present_servos;
+    if(servo_id>MAX_NUM_SERVOS)
+    {
+      throw CDarwinRobotException(_HERE_,"Invalid servo identifier");
+    }
+    // get the current value
+    this->robot_device->read_byte_register(MMANAGER_ENABLE+servo_id/2,&value);
+    if(servo_id%2)// odd servo
+    {
+      if(value&MMANAGER_ODD_SER_EN)
+        return true;
+      else
+        return false;
+    }
+    else// even servo
+    {
+      if(value&MMANAGER_EVEN_SER_EN)
+        return true;
+      else
+        return false;
+    }
   }
   else
     throw CDarwinRobotException(_HERE_,"Invalid robot device");
 }
 
-void CDarwinRobot::mm_start(void)
+void CDarwinRobot::mm_assign_module(unsigned char servo_id, mm_mode_t mode)
 {
+  unsigned char value;
+
   if(this->robot_device!=NULL)
   {
-    this->manager_status|=MANAGER_ENABLE;
-    this->robot_device->write_byte_register(DARWIN_MM_CNTRL,this->manager_status);
+    if(servo_id>MAX_NUM_SERVOS)
+    {
+      throw CDarwinRobotException(_HERE_,"Invalid servo identifier");
+    }
+    // get the current value
+    this->robot_device->read_byte_register(MMANAGER_ENABLE+servo_id/2,&value);
+    if(servo_id%2)// odd servo
+    {
+      value&=(~MMANAGER_ODD_SER_MOD);
+      value|=(unsigned char)mode;
+    }
+    else// even servo
+    {
+      value&=(~MMANAGER_EVEN_SER_MOD);
+      value|=(((unsigned char)mode)<<4);
+    }
+    this->robot_device->write_byte_register(MMANAGER_ENABLE+servo_id/2,value);
   }
   else
     throw CDarwinRobotException(_HERE_,"Invalid robot device");
 }
 
-void CDarwinRobot::mm_stop(void)
+void CDarwinRobot::mm_assign_module(std::string &servo,std::string &module)
 {
+  unsigned int i=0;
+
+  for(i=0;i<MAX_NUM_SERVOS;i++)
+  {
+    if(servo==servo_names[i])
+    {
+      if(module=="action")
+        this->mm_assign_module(i,DARWIN_MM_ACTION);
+      else if(module=="walking")
+        this->mm_assign_module(i,DARWIN_MM_WALKING);
+      else if(module=="joints")
+        this->mm_assign_module(i,DARWIN_MM_JOINTS);
+      else if(module=="head")
+        this->mm_assign_module(i,DARWIN_MM_HEAD);
+      else if(module=="gripper")
+        this->mm_assign_module(i,DARWIN_MM_GRIPPER);
+      else
+        this->mm_assign_module(i,DARWIN_MM_NONE);
+      break;
+    }
+  }
+}
+
+mm_mode_t CDarwinRobot::mm_get_module(unsigned char servo_id)
+{
+  unsigned char value;
+
   if(this->robot_device!=NULL)
   {
-    this->manager_status&=(~MANAGER_ENABLE);
-    this->robot_device->write_byte_register(DARWIN_MM_CNTRL,this->manager_status);
+    if(servo_id>MAX_NUM_SERVOS)
+    {
+      throw CDarwinRobotException(_HERE_,"Invalid servo identifier");
+    }
+    // get the current value
+    this->robot_device->read_byte_register(MMANAGER_ENABLE+servo_id/2,&value);
+    if(servo_id%2)// odd servo
+      return (mm_mode_t)(value&MMANAGER_ODD_SER_MOD);
+    else
+      return (mm_mode_t)((value&MMANAGER_EVEN_SER_MOD)>>4);
   }
   else
     throw CDarwinRobotException(_HERE_,"Invalid robot device");
 }
 
-bool CDarwinRobot::mm_is_running(void)
+/*
+unsigned int CDarwinRobot::mm_get_present_servos(void)
 {
-  unsigned char value;
+  unsigned int present_servos;
 
   if(this->robot_device!=NULL)
   {
-    this->robot_device->read_byte_register(DARWIN_MM_CNTRL,&value);
-    if(value&MANAGER_ENABLE)
-      return true;
-    else
-      return false;
+    this->robot_device->read_registers(DARWIN_MM_PRESENT_SERVOS1,(unsigned char *)&present_servos,4);
+    return present_servos;
   }
   else
     throw CDarwinRobotException(_HERE_,"Invalid robot device");
@@ -665,155 +858,6 @@ fall_t CDarwinRobot::mm_get_fallen_position(void)
     throw CDarwinRobotException(_HERE_,"Invalid robot device");
 }
 
-void CDarwinRobot::mm_enable_servo(unsigned char servo_id)
-{
-  unsigned char value;
-
-  if(this->robot_device!=NULL)
-  {
-    if(servo_id>MAX_NUM_SERVOS)
-    {
-      throw CDarwinRobotException(_HERE_,"Invalid servo identifier");
-    }
-    // get the current value
-    this->robot_device->read_byte_register(DARWIN_MM_MODULE_EN0+servo_id/2,&value);
-    if(servo_id%2)// odd servo
-      value|=MANAGER_ODD_SER_EN;
-    else// even servo
-      value|=MANAGER_EVEN_SER_EN;
-    this->robot_device->write_byte_register(DARWIN_MM_MODULE_EN0+servo_id/2,value);
-  }
-  else
-    throw CDarwinRobotException(_HERE_,"Invalid robot device");
-}
-
-void CDarwinRobot::mm_disable_servo(unsigned char servo_id)
-{
-  unsigned char value;
-
-  if(this->robot_device!=NULL)
-  {
-    if(servo_id>MAX_NUM_SERVOS)
-    {
-      throw CDarwinRobotException(_HERE_,"Invalid servo identifier");
-    }
-    // get the current value
-    this->robot_device->read_byte_register(DARWIN_MM_MODULE_EN0+servo_id/2,&value);
-    if(servo_id%2)// odd servo
-      value&=(~MANAGER_ODD_SER_EN);
-    else// even servo
-      value&=(~MANAGER_EVEN_SER_EN);
-    this->robot_device->write_byte_register(DARWIN_MM_MODULE_EN0+servo_id/2,value);
-  }
-  else
-    throw CDarwinRobotException(_HERE_,"Invalid robot device");
-}
-
-bool CDarwinRobot::mm_is_servo_enabled(unsigned char servo_id)
-{
-  unsigned char value;
-
-  if(this->robot_device!=NULL)
-  {
-    if(servo_id>MAX_NUM_SERVOS)
-    {
-      throw CDarwinRobotException(_HERE_,"Invalid servo identifier");
-    }
-    // get the current value
-    this->robot_device->read_byte_register(DARWIN_MM_MODULE_EN0+servo_id/2,&value);
-    if(servo_id%2)// odd servo
-    {
-      if(value&MANAGER_ODD_SER_EN)
-        return true;
-      else
-        return false;
-    }
-    else// even servo
-    {
-      if(value&MANAGER_EVEN_SER_EN)
-        return true;
-      else
-        return false;
-    }
-  }
-  else
-    throw CDarwinRobotException(_HERE_,"Invalid robot device");
-}
-
-void CDarwinRobot::mm_assign_module(unsigned char servo_id, mm_mode_t mode)
-{
-  unsigned char value;
-
-  if(this->robot_device!=NULL)
-  {
-    if(servo_id>MAX_NUM_SERVOS)
-    {
-      throw CDarwinRobotException(_HERE_,"Invalid servo identifier");
-    }
-    // get the current value
-    this->robot_device->read_byte_register(DARWIN_MM_MODULE_EN0+servo_id/2,&value);
-    if(servo_id%2)// odd servo
-    {
-      value&=(~MANAGER_ODD_SER_MOD);
-      value|=(unsigned char)mode;
-    }
-    else// even servo
-    {
-      value&=(~MANAGER_EVEN_SER_MOD);
-      value|=(((unsigned char)mode)<<4);
-    }
-    this->robot_device->write_byte_register(DARWIN_MM_MODULE_EN0+servo_id/2,value);
-  }
-  else
-    throw CDarwinRobotException(_HERE_,"Invalid robot device");
-}
-
-void CDarwinRobot::mm_assign_module(std::string &servo,std::string &module)
-{
-  unsigned int i=0;
-
-  for(i=0;i<MAX_NUM_SERVOS;i++)
-  {
-    if(servo==servo_names[i])
-    {
-      if(module=="action")
-        this->mm_assign_module(i,DARWIN_MM_ACTION);
-      else if(module=="walking")
-        this->mm_assign_module(i,DARWIN_MM_WALKING);
-      else if(module=="joints")
-        this->mm_assign_module(i,DARWIN_MM_JOINTS);
-      else if(module=="head")
-        this->mm_assign_module(i,DARWIN_MM_HEAD);
-      else if(module=="gripper")
-        this->mm_assign_module(i,DARWIN_MM_GRIPPER);
-      else
-        this->mm_assign_module(i,DARWIN_MM_NONE);
-      break;
-    }
-  }
-}
-
-mm_mode_t CDarwinRobot::mm_get_module(unsigned char servo_id)
-{
-  unsigned char value;
-
-  if(this->robot_device!=NULL)
-  {
-    if(servo_id>MAX_NUM_SERVOS)
-    {
-      throw CDarwinRobotException(_HERE_,"Invalid servo identifier");
-    }
-    // get the current value
-    this->robot_device->read_byte_register(DARWIN_MM_MODULE_EN0+servo_id/2,&value);
-    if(servo_id%2)// odd servo
-      return (mm_mode_t)(value&MANAGER_ODD_SER_MOD);
-    else
-      return (mm_mode_t)((value&MANAGER_EVEN_SER_MOD)>>4);
-  }
-  else
-    throw CDarwinRobotException(_HERE_,"Invalid robot device");
-}
-
 void CDarwinRobot::mm_load_config(std::string &filename)
 {
   darwin_config_t::servo_iterator iterator;
@@ -935,8 +979,29 @@ void CDarwinRobot::mm_get_balance_gains(double *knee,double *ankle_pitch,double
 {
 
 }
-
+*/
 // motion action interface
+unsigned int CDarwinRobot::action_get_num_models(void)
+{
+  unsigned char num;
+
+  if(this->robot_device==NULL)
+    throw CDarwinRobotException(_HERE_,"Invalid robot device");
+  this->robot_device->read_byte_register(MANAGER_NUM_MODULES,&num);
+  return num;
+}
+
+unsigned int CDarwinRobot::action_get_num_devices(void)
+{
+  unsigned char num;
+
+  if(this->robot_device==NULL)
+    throw CDarwinRobotException(_HERE_,"Invalid robot device");
+  this->robot_device->read_byte_register(MANAGER_NUM_MODULES,&num);
+  return num;
+}
+
+/*
 void CDarwinRobot::action_load_page(unsigned char page_id)
 {
   if(this->robot_device!=NULL)
@@ -1010,8 +1075,9 @@ bool CDarwinRobot::action_is_page_running(void)
   else
     throw CDarwinRobotException(_HERE_,"Invalid robot device");
 }
-
+*/
 /* walking interface */
+/*
 void CDarwinRobot::walk_set_x_offset(double offset_m)
 {
   unsigned char offset;
@@ -1580,8 +1646,9 @@ double CDarwinRobot::walk_get_turn_step(void)
   else
     throw CDarwinRobotException(_HERE_,"Invalid robot device");
 }
-
+*/
 // stairs interface
+/*
 void CDarwinRobot::stairs_go_up(void)
 {
   if(this->robot_device!=NULL)
@@ -2135,8 +2202,9 @@ double CDarwinRobot::stairs_get_x_shift_body(void)
   else
     throw CDarwinRobotException(_HERE_,"Invalid robot device");
 }
-
+*/
 // joint motion interface
+/*
 void CDarwinRobot::joints_load(joints_grp_t group,std::vector<unsigned char> &servos,std::vector<double> &angles,std::vector<double> &speeds,std::vector<double> &accels)
 {
   unsigned int group_servos=0,i;
@@ -2215,8 +2283,9 @@ bool CDarwinRobot::joints_are_moving(joints_grp_t group)
   else
     throw CDarwinRobotException(_HERE_,"Invalid robot device");
 }
-
+*/
 // head tracking interface
+/*
 void CDarwinRobot::head_set_pan_pid(double p,double i,double d,double i_clamp)
 {
   unsigned short int value;
@@ -2430,9 +2499,10 @@ void CDarwinRobot::head_get_current_target(double *pan,double *tilt)
   else
     throw CDarwinRobotException(_HERE_,"Invalid robot device");
 }
-
+*/
 /* Smart charger interface */
 // Enable smart charger module
+/*
 void CDarwinRobot::smart_charger_enable(void)
 {
   if(this->robot_device!=NULL)
@@ -2574,8 +2644,9 @@ void CDarwinRobot::smart_charger_set_limit_current(double limit_current)
   else
     throw CDarwinRobotException(_HERE_,"Invalid robot device");
 }
-
+*/
 /* grippers interface */
+/*
 void CDarwinRobot::gripper_set_max_angle(grippers_t gripper_id,double max_angle)
 {
   unsigned short int value;
@@ -2824,7 +2895,7 @@ bool CDarwinRobot::gripper_is_close(grippers_t gripper_id)
   else
     throw CDarwinRobotException(_HERE_,"Invalid robot device");
 }
-
+*/
 CDarwinRobot::~CDarwinRobot()
 {
   if(this->robot_device!=NULL)
diff --git a/src/darwin_robot.h b/src/darwin_robot.h
index 632c73bee677001a788e56e13599d29fbffcf87c..1dcd71f51cbcf05583683b9bf7e475e4181bc139 100644
--- a/src/darwin_robot.h
+++ b/src/darwin_robot.h
@@ -6,7 +6,7 @@
 #include "dynamixelserver_serial.h"
 #include "darwin_registers.h"
 
-#define           MAX_NUM_SERVOS        31
+#define           MAX_NUM_SERVOS        32
 
 extern const std::string servo_names[MAX_NUM_SERVOS];
 /* available motion modules */
@@ -14,44 +14,44 @@ typedef enum {DARWIN_MM_NONE=0x00,DARWIN_MM_ACTION=0x01,DARWIN_MM_WALKING=0x02,D
 /* available grippers */
 typedef enum {LEFT_GRIPPER=0,RIGHT_GRIPPER=1} grippers_t;
 /* available leds */
-#define           GPIO_NUM_LEDS         11
+//#define           GPIO_NUM_LEDS         11
 
-typedef enum {LED_TX=0,LED_RX=1,LED_2=2,LED_3=3,LED_4=4,LED_5_R=5,LED_5_G=6,LED_5_B=7,LED_6_R=8,LED_6_G=9,LED_6_B=10} led_t;
+//typedef enum {LED_TX=0,LED_RX=1,LED_2=2,LED_3=3,LED_4=4,LED_5_R=5,LED_5_G=6,LED_5_B=7,LED_6_R=8,LED_6_G=9,LED_6_B=10} led_t;
 
-typedef struct
-{
-  unsigned short int control_reg;
-  unsigned short int data_reg;
-  unsigned char value_mask;
-  unsigned char toggle_mask;
-  unsigned char blink_mask;
-}led_info_t;
+//typedef struct
+//{
+//  unsigned short int control_reg;
+//  unsigned short int data_reg;
+//  unsigned char value_mask;
+//  unsigned char toggle_mask;
+//  unsigned char blink_mask;
+//}led_info_t;
 
 /* available pushbuttons */
-#define           GPIO_NUM_PB           2
+//#define           GPIO_NUM_PB           2
 
-typedef enum {START_PB=0,MODE_PB=1} pushbutton_t;
+//typedef enum {START_PB=0,MODE_PB=1} pushbutton_t;
 
-#define           ADC_NUM_CHANNELS      12
+//#define           ADC_NUM_CHANNELS      12
 
-typedef enum {ADC_CH1=0,ADC_CH2=1,ADC_CH3=2,ADC_CH4=3,ADC_CH5=4,ADC_CH6=5,ADC_CH7=6,ADC_CH8=7,
-              ADC_CH9=8,ADC_CH10=9,ADC_CH12=10} adc_t;
+//typedef enum {ADC_CH1=0,ADC_CH2=1,ADC_CH3=2,ADC_CH4=3,ADC_CH5=4,ADC_CH6=5,ADC_CH7=6,ADC_CH8=7,
+//              ADC_CH9=8,ADC_CH10=9,ADC_CH12=10} adc_t;
 
-#define           JOINTS_NUM_GROUPS     4
+//#define           JOINTS_NUM_GROUPS     4
 
-typedef enum {JOINTS_GRP0=0,JOINTS_GRP1=1,JOINTS_GRP2=2,JOINTS_GRP3=2} joints_grp_t;
+//typedef enum {JOINTS_GRP0=0,JOINTS_GRP1=1,JOINTS_GRP2=2,JOINTS_GRP3=2} joints_grp_t;
 
-typedef enum {SHIFT_WEIGHT_LEFT=0x10,RISE_RIGHT_FOOT=0x20,ADVANCE_RIGHT_FOOT=0x30,CONTACT_RIGHT_FOOT=0x40,SHIFT_WEIGHT_RIGHT=0x50,RISE_LEFT_FOOT=0x60,ADVANCE_LEFT_FOOT=0x70,CONTACT_LEFT_FOOT=0x80,CENTER_WEIGHT=0x90} stairs_phase_t;
+//typedef enum {SHIFT_WEIGHT_LEFT=0x10,RISE_RIGHT_FOOT=0x20,ADVANCE_RIGHT_FOOT=0x30,CONTACT_RIGHT_FOOT=0x40,SHIFT_WEIGHT_RIGHT=0x50,RISE_LEFT_FOOT=0x60,ADVANCE_LEFT_FOOT=0x70,CONTACT_LEFT_FOOT=0x80,CENTER_WEIGHT=0x90} stairs_phase_t;
 
 //smart charger read data
-#pragma pack (push, 1)
-typedef struct
-{ 
-  unsigned short int avg_time_empty;
-  unsigned short int avg_time_full;
-  unsigned short int bat_status;
-}TChargerData;
-#pragma pack (pop)
+//#pragma pack (push, 1)
+//typedef struct
+//{ 
+//  unsigned short int avg_time_empty;
+//  unsigned short int avg_time_full;
+//  unsigned short int bat_status;
+//}TChargerData;
+//#pragma pack (pop)
 
 typedef enum {MM_FWD_FALL=0,MM_BWD_FALL=1,MM_STANDING=2} fall_t;
 
@@ -63,76 +63,86 @@ class CDarwinRobot
     std::string robot_name;
     unsigned char robot_id;
     // motion manager variables
+    double manager_period;
     unsigned char manager_num_servos;
     unsigned char manager_status;
     /* action status */
     unsigned char action_status;
     //smart charger variables
-    unsigned char smart_charger_status;
-    double min_limit_current;
-    double max_limit_current;
+//    unsigned char smart_charger_status;
+//    double min_limit_current;
+//    double max_limit_current;
   public:
     CDarwinRobot(const std::string &name,std::string &bus_id,int bus_speed, unsigned char id,bool sim=false);
     // GPIO interface
-    void gpio_set_led(led_t led);
-    void gpio_clear_led(led_t led);
-    void gpio_toggle_led(led_t led);
-    void gpio_blink_led(led_t led, int period_ms);
-    void gpio_aux1_color(double red, double green, double blue);
-    void gpio_aux2_color(double red, double green, double blue);
-    bool is_button_pressed(pushbutton_t pushbutton);
+//    void gpio_set_led(led_t led);
+//    void gpio_clear_led(led_t led);
+//    void gpio_toggle_led(led_t led);
+//    void gpio_blink_led(led_t led, int period_ms);
+//    void gpio_aux1_color(double red, double green, double blue);
+//    void gpio_aux2_color(double red, double green, double blue);
+//    bool is_button_pressed(pushbutton_t pushbutton);
     // ADC interface
-    void adc_start(void);
-    bool adc_is_running(void);
-    void adc_set_period(unsigned char period_ms);
-    double adc_get_value(adc_t adc);
-    double adc_get_temperature(void);
-    void adc_stop(void);
+//    void adc_start(void);
+//    bool adc_is_running(void);
+//    void adc_set_period(unsigned char period_ms);
+//    double adc_get_value(adc_t adc);
+//    double adc_get_temperature(void);
+//    void adc_stop(void);
     // imu interface
-    void imu_start(void);
-    void imu_stop(void);
-    void imu_start_gyro_cal(void);
-    void imu_set_cal_samples(unsigned short int num_samples);
-    unsigned short int imu_get_cal_samples(void);
-    bool imu_is_gyro_cal_done(void);
-    bool imu_is_accel_detected(void);
-    bool imu_is_gyro_detected(void);
-    void imu_get_accel_data(double *x,double *y,double *z);
-    void imu_get_gyro_data(double *x,double *y,double *z);
+//    void imu_start(void);
+//    void imu_stop(void);
+//    void imu_start_gyro_cal(void);
+//    void imu_set_cal_samples(unsigned short int num_samples);
+//    unsigned short int imu_get_cal_samples(void);
+//    bool imu_is_gyro_cal_done(void);
+//    bool imu_is_accel_detected(void);
+//    bool imu_is_gyro_detected(void);
+//    void imu_get_accel_data(double *x,double *y,double *z);
+//    void imu_get_gyro_data(double *x,double *y,double *z);
+    // dynamixel manager interface
+    double manager_get_period(void);
+    void manager_set_period(double period_s);
+    unsigned int manager_get_num_modules(void);
+    unsigned int managet_get_num_masters(void);
+    void manager_start(void);
+    void manager_stop(void);
+    bool manager_is_running(void);
+    void manager_start_scan(void);
+    bool manager_is_scanning(void);
+    unsigned char manager_get_num_devices(void);
     // motion manager interface
-    void mm_set_period(double period_ms);
+    void mm_set_period(double period_s);
     double mm_get_period(void);
-    unsigned char mm_get_num_servos(void);
-    unsigned int mm_get_present_servos(void);
-    void mm_start(void);
-    void mm_stop(void);
-    bool mm_is_running(void);
-    void mm_enable_balance(void);
-    void mm_disable_balance(void);
-    bool mm_is_balance_enabled(void);
-    void mm_enable_power(void);
-    void mm_disable_power(void);
-    bool mm_is_power_enabled(void);
-    void mm_enable_power_v2(void);
-    void mm_disable_power_v2(void);
-    bool mm_is_power_enabled_v2(void);
-    bool mm_has_fallen(void);
-    fall_t mm_get_fallen_position(void);
     void mm_enable_servo(unsigned char servo_id);
     void mm_disable_servo(unsigned char servo_id);
     bool mm_is_servo_enabled(unsigned char servo_id);
     void mm_assign_module(unsigned char servo_id, mm_mode_t mode);
     void mm_assign_module(std::string &servo,std::string &module);
     mm_mode_t mm_get_module(unsigned char servo_id);
-    std::vector<double> mm_get_servo_angles(void);
-    double mm_get_servo_angle(unsigned char servo_id);
-    void mm_set_servo_offset(unsigned char servo_id,double offset);
-    double mm_get_servo_offset(unsigned char servo_id);
-    std::vector<double> mm_get_servo_offsets(void);
-    void mm_load_config(std::string &filename);
-    void mm_set_balance_gains(double knee,double ankle_pitch,double hip_roll,double ankle_roll);
-    void mm_get_balance_gains(double *knee,double *ankle_pitch,double *hip_roll,double *ankle_roll);
+//    unsigned int mm_get_present_servos(void);
+//    void mm_enable_balance(void);
+//    void mm_disable_balance(void);
+//    bool mm_is_balance_enabled(void);
+//    void mm_enable_power(void);
+//    void mm_disable_power(void);
+//    bool mm_is_power_enabled(void);
+//    void mm_enable_power_v2(void);
+//    void mm_disable_power_v2(void);
+//    bool mm_is_power_enabled_v2(void);
+//    bool mm_has_fallen(void);
+//    fall_t mm_get_fallen_position(void);
+//    std::vector<double> mm_get_servo_angles(void);
+//    double mm_get_servo_angle(unsigned char servo_id);
+//    void mm_set_servo_offset(unsigned char servo_id,double offset);
+//    double mm_get_servo_offset(unsigned char servo_id);
+//    std::vector<double> mm_get_servo_offsets(void);
+//    void mm_load_config(std::string &filename);
+//    void mm_set_balance_gains(double knee,double ankle_pitch,double hip_roll,double ankle_roll);
+//    void mm_get_balance_gains(double *knee,double *ankle_pitch,double *hip_roll,double *ankle_roll);
     // motion action interface
+    unsigned int action_get_num_models(void);
+    unsigned int action_get_num_devices(void);
     void action_load_page(unsigned char page_id);
     unsigned char action_get_current_page(void);
     unsigned char action_get_current_step(void);
@@ -140,139 +150,139 @@ class CDarwinRobot
     void action_stop(void);
     bool action_is_page_running(void);
     /* walking interface */
-    void walk_set_x_offset(double offset_m);
-    double walk_get_x_offset(void);
-    void walk_set_y_offset(double offset_m);
-    double walk_get_y_offset(void);
-    void walk_set_z_offset(double offset_m);
-    double walk_get_z_offset(void);
-    void walk_set_roll_offset(double offset_rad);
-    double walk_get_roll_offset(void);
-    void walk_set_pitch_offset(double offset_rad);
-    double walk_get_pitch_offset(void);
-    void walk_set_yaw_offset(double offset_rad);
-    double walk_get_yaw_offset(void);
-    void walk_set_hip_pitch_offset(double offset_rad);
-    double walk_get_hip_pitch_offset(void);
-    void walk_set_period(double period_s);
-    double walk_get_period(void);
-    void walk_set_ssp_dsp_ratio(double ratio);
-    double walk_get_ssp_dsp_ratio(void);
-    void walk_set_fwd_bwd_ratio(double ratio);
-    double walk_get_fwd_bwd_ratio(void);
-    void walk_set_foot_height(double height_m);
-    double walk_get_foot_height(void);
-    void walk_set_left_right_swing(double swing_m);
-    double walk_get_left_right_swing(void);
-    void walk_set_top_down_swing(double swing_m);
-    double walk_get_top_down_swing(void);
-    void walk_set_pelvis_offset(double offset_rad);
-    double walk_get_pelvis_offset(void);
-    void walk_set_arm_swing_gain(double gain);
-    double walk_get_arm_swing_gain(void);
-    void walk_set_trans_speed(double speed_m_s);
-    double walk_get_trans_speed(void);
-    void walk_set_rot_speed(double speed_rad_s);
-    double walk_get_rot_speed(void);
-    void walk_start(void);
-    void walk_stop(void);
-    bool is_walking(void);
-    void walk_set_x_step(double step_m);
-    double walk_get_x_step(void);
-    void walk_set_y_step(double step_m);
-    double walk_get_y_step(void);
-    void walk_set_turn_step(double step_rad);
-    double walk_get_turn_step(void);
+//    void walk_set_x_offset(double offset_m);
+//    double walk_get_x_offset(void);
+//    void walk_set_y_offset(double offset_m);
+//    double walk_get_y_offset(void);
+//    void walk_set_z_offset(double offset_m);
+//    double walk_get_z_offset(void);
+//    void walk_set_roll_offset(double offset_rad);
+//    double walk_get_roll_offset(void);
+//    void walk_set_pitch_offset(double offset_rad);
+//    double walk_get_pitch_offset(void);
+//    void walk_set_yaw_offset(double offset_rad);
+//    double walk_get_yaw_offset(void);
+//    void walk_set_hip_pitch_offset(double offset_rad);
+//    double walk_get_hip_pitch_offset(void);
+//    void walk_set_period(double period_s);
+//    double walk_get_period(void);
+//    void walk_set_ssp_dsp_ratio(double ratio);
+//    double walk_get_ssp_dsp_ratio(void);
+//    void walk_set_fwd_bwd_ratio(double ratio);
+//    double walk_get_fwd_bwd_ratio(void);
+//    void walk_set_foot_height(double height_m);
+//    double walk_get_foot_height(void);
+//    void walk_set_left_right_swing(double swing_m);
+//    double walk_get_left_right_swing(void);
+//    void walk_set_top_down_swing(double swing_m);
+//    double walk_get_top_down_swing(void);
+//    void walk_set_pelvis_offset(double offset_rad);
+//    double walk_get_pelvis_offset(void);
+//    void walk_set_arm_swing_gain(double gain);
+//    double walk_get_arm_swing_gain(void);
+//    void walk_set_trans_speed(double speed_m_s);
+//    double walk_get_trans_speed(void);
+//    void walk_set_rot_speed(double speed_rad_s);
+//    double walk_get_rot_speed(void);
+//    void walk_start(void);
+//    void walk_stop(void);
+//    bool is_walking(void);
+//    void walk_set_x_step(double step_m);
+//    double walk_get_x_step(void);
+//    void walk_set_y_step(double step_m);
+//    double walk_get_y_step(void);
+//    void walk_set_turn_step(double step_rad);
+//    double walk_get_turn_step(void);
     // stairs interface
-    void stairs_go_up(void); 
-    void stairs_go_down(void);
-    void stairs_stop(void);
-    bool stairs_is_active(void);
-    stairs_phase_t stairs_get_phase(void);
-    void stairs_set_phase_time(stairs_phase_t phase_id,double time);
-    double stairs_get_phase_time(stairs_phase_t phase_id);
-    void stairs_set_x_offset(double offset_m);
-    double stairs_get_x_offset(void);
-    void stairs_set_y_offset(double offset_m);
-    double stairs_get_y_offset(void);
-    void stairs_set_z_offset(double offset_m);
-    double stairs_get_z_offset(void);
-    void stairs_set_roll_offset(double offset_rad);
-    double stairs_get_roll_offset(void);
-    void stairs_set_pitch_offset(double offset_rad);
-    double stairs_get_pitch_offset(void);
-    void stairs_set_yaw_offset(double offset_rad);
-    double stairs_get_yaw_offset(void);
-    void stairs_set_y_shift(double distance);
-    double stairs_get_y_shift(void);
-    void stairs_set_x_shift(double distance);
-    double stairs_get_x_shift(void);
-    void stairs_set_z_overshoot(double distance);
-    double stairs_get_z_overshoot(void);
-    void stairs_set_z_height(double distance);
-    double stairs_get_z_height(void);
-    void stairs_set_hip_pitch_offset(double offset_rad);
-    double stairs_get_hip_pitch_offset(void);
-    void stairs_set_roll_shift(double angle);
-    double stairs_get_roll_shift(void);
-    void stairs_set_pitch_shift(double angle);
-    double stairs_get_pitch_shift(void);
-    void stairs_set_yaw_shift(double angle);
-    double stairs_get_yaw_shift(void);
-    void stairs_set_y_spread(double distance);
-    double stairs_get_y_spread(void);
-    void stairs_set_x_shift_body(double distance);
-    double stairs_get_x_shift_body(void);
+//    void stairs_go_up(void); 
+//    void stairs_go_down(void);
+//    void stairs_stop(void);
+//    bool stairs_is_active(void);
+//    stairs_phase_t stairs_get_phase(void);
+//    void stairs_set_phase_time(stairs_phase_t phase_id,double time);
+//    double stairs_get_phase_time(stairs_phase_t phase_id);
+//    void stairs_set_x_offset(double offset_m);
+//    double stairs_get_x_offset(void);
+//    void stairs_set_y_offset(double offset_m);
+//    double stairs_get_y_offset(void);
+//    void stairs_set_z_offset(double offset_m);
+//    double stairs_get_z_offset(void);
+//    void stairs_set_roll_offset(double offset_rad);
+//    double stairs_get_roll_offset(void);
+//    void stairs_set_pitch_offset(double offset_rad);
+//    double stairs_get_pitch_offset(void);
+//    void stairs_set_yaw_offset(double offset_rad);
+//    double stairs_get_yaw_offset(void);
+//    void stairs_set_y_shift(double distance);
+//    double stairs_get_y_shift(void);
+//    void stairs_set_x_shift(double distance);
+//    double stairs_get_x_shift(void);
+//    void stairs_set_z_overshoot(double distance);
+//    double stairs_get_z_overshoot(void);
+//    void stairs_set_z_height(double distance);
+//    double stairs_get_z_height(void);
+//    void stairs_set_hip_pitch_offset(double offset_rad);
+//    double stairs_get_hip_pitch_offset(void);
+//    void stairs_set_roll_shift(double angle);
+//    double stairs_get_roll_shift(void);
+//    void stairs_set_pitch_shift(double angle);
+//    double stairs_get_pitch_shift(void);
+//    void stairs_set_yaw_shift(double angle);
+//    double stairs_get_yaw_shift(void);
+//    void stairs_set_y_spread(double distance);
+//    double stairs_get_y_spread(void);
+//    void stairs_set_x_shift_body(double distance);
+//    double stairs_get_x_shift_body(void);
     // joint motion interface
-    void joints_load(joints_grp_t group,std::vector<unsigned char> &servos,std::vector<double> &angles,std::vector<double> &speeds,std::vector<double> &accels);
-    std::vector<unsigned char> joints_get_group_servos(joints_grp_t group);
-    std::vector<double> joints_get_group_angles(joints_grp_t group);
-    std::vector<double> joints_get_group_speeds(joints_grp_t group);
-    std::vector<double> joints_get_group_accels(joints_grp_t group);
-    void joints_start(joints_grp_t group);
-    void joints_stop(joints_grp_t group);
-    bool joints_are_moving(joints_grp_t group);
+//    void joints_load(joints_grp_t group,std::vector<unsigned char> &servos,std::vector<double> &angles,std::vector<double> &speeds,std::vector<double> &accels);
+//    std::vector<unsigned char> joints_get_group_servos(joints_grp_t group);
+//    std::vector<double> joints_get_group_angles(joints_grp_t group);
+//    std::vector<double> joints_get_group_speeds(joints_grp_t group);
+//    std::vector<double> joints_get_group_accels(joints_grp_t group);
+//    void joints_start(joints_grp_t group);
+//    void joints_stop(joints_grp_t group);
+//    bool joints_are_moving(joints_grp_t group);
     // head tracking interface
-    void head_set_pan_pid(double p,double i,double d,double i_clamp);
-    void head_get_pan_pid(double *p,double *i,double *d,double *i_clamp);
-    void head_set_tilt_pid(double p,double i,double d,double i_clamp);
-    void head_get_tilt_pid(double *p,double *i,double *d,double *i_clamp);
-    void head_set_pan_range(double max,double min);
-    void head_get_pan_range(double *max,double *min);
-    void head_set_tilt_range(double max,double min);
-    void head_get_tilt_range(double *max,double *min);
-    void head_start_tracking(double pan,double tilt);
-    void head_stop_tracking(void);
-    bool head_is_tracking(void);
-    void head_set_new_target(double pan,double tilt);
-    void head_get_current_target(double *pan,double *tilt);
+//    void head_set_pan_pid(double p,double i,double d,double i_clamp);
+//    void head_get_pan_pid(double *p,double *i,double *d,double *i_clamp);
+//    void head_set_tilt_pid(double p,double i,double d,double i_clamp);
+//    void head_get_tilt_pid(double *p,double *i,double *d,double *i_clamp);
+//    void head_set_pan_range(double max,double min);
+//    void head_get_pan_range(double *max,double *min);
+//    void head_set_tilt_range(double max,double min);
+//    void head_get_tilt_range(double *max,double *min);
+//    void head_start_tracking(double pan,double tilt);
+//    void head_stop_tracking(void);
+//    bool head_is_tracking(void);
+//    void head_set_new_target(double pan,double tilt);
+//    void head_get_current_target(double *pan,double *tilt);
     
     // smart charger interface
     /** 
      * \brief Function to check if smart charger module is detected
      */
-    bool is_smart_charger_detected(void);
+//    bool is_smart_charger_detected(void);
     /**
      * \brief Function to check if smart charger module is enabled
      */
-    bool is_smart_charger_enabled(void);
+//    bool is_smart_charger_enabled(void);
     /**
      * \brief Function to enable smart charger module
      */
-    void smart_charger_enable(void);
+//    void smart_charger_enable(void);
     /**
      * \brief Function to disable smart charger module
      */
-    void smart_charger_disable(void);
+//    void smart_charger_disable(void);
     /**
      * \brief Function to set smart charger's read operation period
      * \param period Period in s of smart charger module
      */
-    void smart_charger_set_period(double period);
+//    void smart_charger_set_period(double period);
     /**
      * \brief Function to get smart charger's read operation period in segs
      */
-    double smart_charger_get_period(void);
+//    double smart_charger_get_period(void);
     /**
      * \brief Function to get smart charger's data: Battery average time to empty and to full and battery status
      * 
@@ -280,35 +290,35 @@ class CDarwinRobot
      * Avg time to full
      * Avg time to empty
      */
-    TChargerData smart_charger_get_data(void);
+//    TChargerData smart_charger_get_data(void);
     /**
      * 
      */
-    void smart_charger_set_range_current(double min_current, double max_current);
+//    void smart_charger_set_range_current(double min_current, double max_current);
     /**
      * \brief Function to get limit current in A
      */
-    double smart_charger_get_limit_current(void);
+//    double smart_charger_get_limit_current(void);
     /**
      * \brief Function to set smart charger's limit current
      * \param limit_current Value of limit current in A
     */
-    void smart_charger_set_limit_current(double limit_current);
+//    void smart_charger_set_limit_current(double limit_current);
    
     /* grippers interface */
-    void gripper_set_max_angle(grippers_t gripper_id,double max_angle);
-    double gripper_get_max_angle(grippers_t gripper_id);
-    void gripper_set_min_angle(grippers_t gripper_id,double min_angle);
-    double gripper_get_min_angle(grippers_t gripper_id);
-    void gripper_set_max_force(grippers_t gripper_id,unsigned short int max_force);
-    unsigned short int gripper_get_max_force(grippers_t gripper_id);
-    void gripper_set_servo_ids(grippers_t gripper_id, unsigned char top_id, unsigned char bot_id);
-    void gripper_get_servo_ids(grippers_t gripper_id, unsigned char *top_id, unsigned char *bot_id);
-    void gripper_open(grippers_t gripper_id);
-    void gripper_close(grippers_t gripper_id);
-    bool gripper_is_moving(grippers_t gripper_id);
-    bool gripper_is_open(grippers_t gripper_id);
-    bool gripper_is_close(grippers_t gripper_id);
+//    void gripper_set_max_angle(grippers_t gripper_id,double max_angle);
+//    double gripper_get_max_angle(grippers_t gripper_id);
+//    void gripper_set_min_angle(grippers_t gripper_id,double min_angle);
+//    double gripper_get_min_angle(grippers_t gripper_id);
+//    void gripper_set_max_force(grippers_t gripper_id,unsigned short int max_force);
+//    unsigned short int gripper_get_max_force(grippers_t gripper_id);
+//    void gripper_set_servo_ids(grippers_t gripper_id, unsigned char top_id, unsigned char bot_id);
+//    void gripper_get_servo_ids(grippers_t gripper_id, unsigned char *top_id, unsigned char *bot_id);
+//    void gripper_open(grippers_t gripper_id);
+//    void gripper_close(grippers_t gripper_id);
+//    bool gripper_is_moving(grippers_t gripper_id);
+//    bool gripper_is_open(grippers_t gripper_id);
+//    bool gripper_is_close(grippers_t gripper_id);
     ~CDarwinRobot();
 };
 
diff --git a/src/examples/CMakeLists.txt b/src/examples/CMakeLists.txt
index ff70e9bc45b314d70850cd3f631a14fa076eaa93..d598831db0e279dd9b08c6e94b27239b6296169a 100644
--- a/src/examples/CMakeLists.txt
+++ b/src/examples/CMakeLists.txt
@@ -4,63 +4,63 @@ ADD_EXECUTABLE(darwin_manager_test darwin_manager_test.cpp)
 TARGET_LINK_LIBRARIES(darwin_manager_test darwin_robot)
 
 # create an example application
-ADD_EXECUTABLE(darwin_gpio_test darwin_gpio_test.cpp)
+#ADD_EXECUTABLE(darwin_gpio_test darwin_gpio_test.cpp)
 # link necessary libraries
-TARGET_LINK_LIBRARIES(darwin_gpio_test darwin_robot)
+#TARGET_LINK_LIBRARIES(darwin_gpio_test darwin_robot)
 
 # create an example application
-ADD_EXECUTABLE(darwin_adc_test darwin_adc_test.cpp)
+#ADD_EXECUTABLE(darwin_adc_test darwin_adc_test.cpp)
 # link necessary libraries
-TARGET_LINK_LIBRARIES(darwin_adc_test darwin_robot)
+#TARGET_LINK_LIBRARIES(darwin_adc_test darwin_robot)
 
 # create an example application
-ADD_EXECUTABLE(darwin_imu_test darwin_imu_test.cpp)
+#ADD_EXECUTABLE(darwin_imu_test darwin_imu_test.cpp)
 # link necessary libraries
-TARGET_LINK_LIBRARIES(darwin_imu_test darwin_robot)
+#TARGET_LINK_LIBRARIES(darwin_imu_test darwin_robot)
 
 # create an example application
-ADD_EXECUTABLE(darwin_action_test darwin_action_test.cpp)
+#ADD_EXECUTABLE(darwin_action_test darwin_action_test.cpp)
 # link necessary libraries
-TARGET_LINK_LIBRARIES(darwin_action_test darwin_robot)
+#TARGET_LINK_LIBRARIES(darwin_action_test darwin_robot)
 
 # create an example application
-ADD_EXECUTABLE(darwin_walking_test darwin_walking_test.cpp)
+#ADD_EXECUTABLE(darwin_walking_test darwin_walking_test.cpp)
 # link necessary libraries
-TARGET_LINK_LIBRARIES(darwin_walking_test darwin_robot)
+#TARGET_LINK_LIBRARIES(darwin_walking_test darwin_robot)
 
 # create an example application
-ADD_EXECUTABLE(darwin_stairs_test darwin_stairs_test.cpp)
+#ADD_EXECUTABLE(darwin_stairs_test darwin_stairs_test.cpp)
 # link necessary libraries
-TARGET_LINK_LIBRARIES(darwin_stairs_test darwin_robot)
+#TARGET_LINK_LIBRARIES(darwin_stairs_test darwin_robot)
 
 # create an example application
-ADD_EXECUTABLE(darwin_joint_motion_test darwin_joint_motion_test.cpp)
+#ADD_EXECUTABLE(darwin_joint_motion_test darwin_joint_motion_test.cpp)
 # link necessary libraries
-TARGET_LINK_LIBRARIES(darwin_joint_motion_test darwin_robot)
+#TARGET_LINK_LIBRARIES(darwin_joint_motion_test darwin_robot)
 
 # create an example application
-ADD_EXECUTABLE(darwin_head_tracking_test darwin_head_tracking_test.cpp)
+#ADD_EXECUTABLE(darwin_head_tracking_test darwin_head_tracking_test.cpp)
 # link necessary libraries
-TARGET_LINK_LIBRARIES(darwin_head_tracking_test darwin_robot)
+#TARGET_LINK_LIBRARIES(darwin_head_tracking_test darwin_robot)
 
-IF(KDL_FOUND)
+#IF(KDL_FOUND)
   # create an example application
-  ADD_EXECUTABLE(darwin_arm_kin darwin_arm_kin.cpp)
+#  ADD_EXECUTABLE(darwin_arm_kin darwin_arm_kin.cpp)
   # link necessary libraries
-  TARGET_LINK_LIBRARIES(darwin_arm_kin darwin_arm_kinematics)
+#  TARGET_LINK_LIBRARIES(darwin_arm_kin darwin_arm_kinematics)
 
   # create an example application
-  ADD_EXECUTABLE(darwin_leg_kin darwin_leg_kin.cpp)
+#  ADD_EXECUTABLE(darwin_leg_kin darwin_leg_kin.cpp)
   # link necessary libraries
-  TARGET_LINK_LIBRARIES(darwin_leg_kin darwin_leg_kinematics)
-ENDIF(KDL_FOUND)
+#  TARGET_LINK_LIBRARIES(darwin_leg_kin darwin_leg_kinematics)
+#ENDIF(KDL_FOUND)
 
 # create an example application
-ADD_EXECUTABLE(darwin_smart_charger_test darwin_smart_charger_test.cpp)
+#ADD_EXECUTABLE(darwin_smart_charger_test darwin_smart_charger_test.cpp)
 # link necessary libraries
-TARGET_LINK_LIBRARIES(darwin_smart_charger_test darwin_robot)
+#TARGET_LINK_LIBRARIES(darwin_smart_charger_test darwin_robot)
 
 # create an example application
-ADD_EXECUTABLE(darwin_gripper_test darwin_gripper_test.cpp)
+#ADD_EXECUTABLE(darwin_gripper_test darwin_gripper_test.cpp)
 # link necessary libraries
-TARGET_LINK_LIBRARIES(darwin_gripper_test darwin_robot)
+#TARGET_LINK_LIBRARIES(darwin_gripper_test darwin_robot)
diff --git a/src/examples/darwin_manager_test.cpp b/src/examples/darwin_manager_test.cpp
index 309308403e0071ec4eec2650c8cd2e4eb81c3767..98b6fc8d3e69572b52170592e0f029b2e0a656fa 100644
--- a/src/examples/darwin_manager_test.cpp
+++ b/src/examples/darwin_manager_test.cpp
@@ -3,25 +3,39 @@
 
 #include <iostream>
 
-std::string robot_device="A603LOBS";
+//std::string robot_device="A603LOBS";
 //std::string robot_device="A4008atn";
+std::string robot_device="/tmp/darwin_driver";
 
 int main(int argc, char *argv[])
 {
-  int i=0,num_servos;
-  unsigned int present_servos;
-  std::vector<int> servos;
-  std::vector<double> angles,speeds,accels,offsets;
+//  int i=0,num_servos;
+//  unsigned int present_servos;
+//  std::vector<int> servos;
+//  std::vector<double> angles,speeds,accels,offsets;
 
   try{
-    CDarwinRobot darwin("Darwin",robot_device,1000000,0x02);
-    num_servos=darwin.mm_get_num_servos();
-    present_servos=darwin.mm_get_present_servos();
-    std::cout << "Found " << num_servos << " servos " << std::endl;
-    std::cout << "Present servos: " << present_servos << std::hex << "0x" << present_servos << std::dec << std::endl;
-    std::cout << "Motion manager period " << darwin.mm_get_period() << " ms" << std::endl;
+    CDarwinRobot darwin("Darwin",robot_device,1000000,0x01,true);
+    std::cout << "Manager period: " << darwin.manager_get_period() << std::endl;
+    std::cout << "Number of modules: " << darwin.manager_get_num_modules() << std::endl;
+    std::cout << "Number of masters: " << darwin.managet_get_num_masters() << std::endl;
+    std::cout << "Motion manager period: " << darwin.mm_get_period() << std::endl;
+    darwin.manager_start();
+    darwin.manager_start_scan();
+    while(darwin.manager_is_scanning())
+    {
+      std::cout << "scanning ..." << std::endl;
+      usleep(100000);
+    }
+    std::cout << "num. devices: " << (int)darwin.manager_get_num_devices() << std::endl;
+//    num_servos=darwin.mm_get_num_servos();
+//    present_servos=darwin.mm_get_present_servos();
+//    std::cout << "Found " << num_servos << " servos " << std::endl;
+//    std::cout << "Present servos: " << present_servos << std::hex << "0x" << present_servos << std::dec << std::endl;
+//    std::cout << "Motion manager period " << darwin.mm_get_period() << " ms" << std::endl;
     // execute the walk ready action
-    std::cout << "Assign servos to the action module" << std::endl;
+//    std::cout << "Assign servos to the action module" << std::endl;
+/*
     for(i=1;i<=20;i++)
     {
       darwin.mm_enable_servo(i);
@@ -88,6 +102,7 @@ int main(int argc, char *argv[])
     offsets=darwin.mm_get_servo_offsets();
     for(i=0;i<MAX_NUM_SERVOS;i++)
       std::cout << "  Servo " << i << ": " << offsets[i] << std::endl;
+*/
   }catch(CException &e){
     std::cout << e.what() << std::endl;
   }