diff --git a/src/darwin_robot.cpp b/src/darwin_robot.cpp
index ea8d1f6201429cb5025ee0d0184809f784c469de..5c5b556409fcaf0d1d726c023ed6103531d37fb1 100644
--- a/src/darwin_robot.cpp
+++ b/src/darwin_robot.cpp
@@ -34,7 +34,11 @@ const std::string servo_names[MAX_NUM_SERVOS]={std::string("Servo0"),
                                                std::string("Servo23"),
                                                std::string("Servo24"),
                                                std::string("Servo25"),
-                                               std::string("Servo26")};
+                                               std::string("Servo26"),
+                                               std::string("Servo27"),
+                                               std::string("Servo28"),
+                                               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},
@@ -64,6 +68,8 @@ CDarwinRobot::CDarwinRobot(const std::string &name,std::string &bus_id,int bus_s
       this->dyn_server->config_bus(bus_id,bus_speed);
       this->robot_device=dyn_server->get_device(id,dyn_version2);
       this->robot_id=id;
+      /* get the current manager status */
+      this->robot_device->read_byte_register(DARWIN_MM_CNTRL,&this->manager_status);
     }
     else
     {
@@ -246,7 +252,6 @@ double CDarwinRobot::adc_get_temperature(void)
   if(this->robot_device!=NULL)
   {
     this->robot_device->read_word_register(DARWIN_ADC_TEMP_L,&value);
-    std::cout << std::hex << value << std::endl;
 
     return ((double)value)/((double)(1<<10));
   }
@@ -403,194 +408,258 @@ void CDarwinRobot::imu_get_gyro_data(double *x,double *y,double *z)
 }
 
 // motion manager interface
-/*void CDarwinRobot::mm_set_period(double period_ms)
+void CDarwinRobot::mm_set_period(double period_ms)
 {
   unsigned short period;
-  // internal time in 0|16 fixed point float format in seconds
-  period=(period_ms/1000.0)*65536;
-  this->robot_device->write_word_register(DARWIN_MM_PERIOD_LOW,period);
+
+  if(this->robot_device!=NULL)
+  {
+    period=(period_ms/1000.0);
+    this->robot_device->write_word_register(DARWIN_MM_PERIOD_L,period);
+  }
+  else
+    throw CDarwinRobotException(_HERE_,"Invalid robot device");
 }
 
 double CDarwinRobot::mm_get_period(void)
 {
   unsigned short period;
 
-  this->robot_device->read_word_register(DARWIN_MM_PERIOD_LOW,&period);
-  return (((double)period)*1000.0)/65536;
+  if(this->robot_device!=NULL)
+  {
+    this->robot_device->read_word_register(DARWIN_MM_PERIOD_L,&period);
+    return ((double)period)*1000.0;
+  }
+  else
+    throw CDarwinRobotException(_HERE_,"Invalid robot device");
 }
 
-double CDarwinRobot::mm_get_current_period(void)
+unsigned char CDarwinRobot::mm_get_num_servos(void)
 {
-  unsigned short period;
-
-  this->robot_device->read_word_register(DARWIN_MM_CUR_PERIOD,&period);
-  return ((double)period)/1000.0;
+  if(this->robot_device!=NULL)
+  {
+    this->robot_device->read_byte_register(DARWIN_MM_NUM_SERVOS,&this->num_servos);
+    return this->num_servos;
+  }
+  else
+    throw CDarwinRobotException(_HERE_,"Invalid robot device");
 }
 
-void CDarwinRobot::mm_get_num_servos(int *v1_servos,int *v2_servos)
+void CDarwinRobot::mm_start(void)
 {
-  unsigned char data;
-
-  this->robot_device->read_byte_register(DARWIN_MM_NUM_SERVOS,&data);
-  *v1_servos=data&0x1F;
-  *v2_servos=(data>>5)&0x07;
+  if(this->robot_device!=NULL)
+  {
+    this->manager_status|=MANAGER_ENABLE;
+    this->robot_device->write_byte_register(DARWIN_MM_CNTRL,this->manager_status);
+  }
+  else
+    throw CDarwinRobotException(_HERE_,"Invalid robot device");
 }
 
-void CDarwinRobot::mm_start(void)
+void CDarwinRobot::mm_stop(void)
 {
-  unsigned char status;
-
-  this->robot_device->read_byte_register(DARWIN_MM_CONTROL,&status);
-  status|=0x01;
-  this->robot_device->write_byte_register(DARWIN_MM_CONTROL,status);
+  if(this->robot_device!=NULL)
+  {
+    this->manager_status&=(~MANAGER_ENABLE);
+    this->robot_device->write_byte_register(DARWIN_MM_CNTRL,this->manager_status);
+  }
+  else
+    throw CDarwinRobotException(_HERE_,"Invalid robot device");
 }
 
-void CDarwinRobot::mm_stop(void)
+bool CDarwinRobot::mm_is_running(void)
 {
-  unsigned char status;
+  unsigned char value;
 
-  this->robot_device->read_byte_register(DARWIN_MM_CONTROL,&status);
-  status&=0xFE;
-  this->robot_device->write_byte_register(DARWIN_MM_CONTROL,status);
+  if(this->robot_device!=NULL)
+  {
+    this->robot_device->read_byte_register(DARWIN_MM_CNTRL,&value);
+    if(value&MANAGER_ENABLE)
+      return true;
+    else
+      return false;
+  }
+  else
+    throw CDarwinRobotException(_HERE_,"Invalid robot device");
 }
 
-void CDarwinRobot::mm_enable_power(void)
+void CDarwinRobot::mm_enable_balance(void)
 {
-  this->robot_device->write_byte_register(DARWIN_DXL_POWER,0x01);
+  if(this->robot_device!=NULL)
+  {
+    this->manager_status|=MANAGER_EN_BAL;
+    this->robot_device->write_byte_register(DARWIN_MM_CNTRL,this->manager_status);
+  }
+  else
+    throw CDarwinRobotException(_HERE_,"Invalid robot device");
 }
 
-void CDarwinRobot::mm_disable_power(void)
+void CDarwinRobot::mm_disable_balance(void)
 {
-  this->robot_device->write_byte_register(DARWIN_DXL_POWER,0x00);
-  sleep(1);
+  if(this->robot_device!=NULL)
+  {
+    this->manager_status&=(~MANAGER_EN_BAL);
+    this->robot_device->write_byte_register(DARWIN_MM_CNTRL,this->manager_status);
+  }
+  else
+    throw CDarwinRobotException(_HERE_,"Invalid robot device");
 }
 
-bool CDarwinRobot::mm_is_power_enabled(void)
+bool CDarwinRobot::mm_is_balance_enabled(void)
 {
   unsigned char value;
 
-  this->robot_device->read_byte_register(DARWIN_DXL_POWER,&value);
-  if(value==0x01)
-    return true;
+  if(this->robot_device!=NULL)
+  {
+    this->robot_device->read_byte_register(DARWIN_MM_CNTRL,&value);
+    if(value&MANAGER_EN_BAL)
+      return true;
+    else
+      return false;
+  }
   else
-    return false;
+    throw CDarwinRobotException(_HERE_,"Invalid robot device");
 }
 
-void CDarwinRobot::mm_enable_servo(unsigned char servo_id)
+void CDarwinRobot::mm_enable_power(void)
 {
-  unsigned char value;
-
-  if(servo_id>MAX_NUM_SERVOS)
+  if(this->robot_device!=NULL)
   {
-    // handle exceptions
-    throw CDarwinRobotException(_HERE_,"Invalid servo identifier");
+    this->manager_status|=MANAGER_EN_PWR;
+    this->robot_device->write_byte_register(DARWIN_MM_CNTRL,this->manager_status);
   }
-  // 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|=0x08;
-  else// even servo
-    value|=0x80;
-  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)
+void CDarwinRobot::mm_disable_power(void)
 {
-  unsigned char value;
-
-  if(servo_id>MAX_NUM_SERVOS)
+  if(this->robot_device!=NULL)
   {
-    // handle exceptions 
-    throw CDarwinRobotException(_HERE_,"Invalid servo identifier");
+    this->manager_status&=(~MANAGER_EN_PWR);
+    this->robot_device->write_byte_register(DARWIN_MM_CNTRL,this->manager_status);
   }
-  // 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&=0xF7;
-  else// even servo
-    value&=0x7F;
-  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)
+bool CDarwinRobot::mm_is_power_enabled(void)
 {
   unsigned char value;
 
-  if(servo_id>MAX_NUM_SERVOS)
-  {
-    // handle exceptions
-    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&0x01)
-      return true;
-    else
-      return false;
-  }
-  else// even servo
+  if(this->robot_device!=NULL)
   {
-    if(value&0x10)
+    this->robot_device->read_byte_register(DARWIN_MM_CNTRL,&value);
+    if(value&MANAGER_EN_PWR)
       return true;
     else
       return false;
   }
+  else
+    throw CDarwinRobotException(_HERE_,"Invalid robot device");
 }
 
-void CDarwinRobot::mm_enable_balance(void)
+void CDarwinRobot::mm_enable_servo(unsigned char servo_id)
 {
-  unsigned char status;
+  unsigned char value;
 
-  this->robot_device->read_byte_register(DARWIN_MM_CONTROL,&status);
-  status|=0x02;
-  this->robot_device->write_byte_register(DARWIN_MM_CONTROL,status);
+  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_balance(void)
+void CDarwinRobot::mm_disable_servo(unsigned char servo_id)
 {
-  unsigned char status;
+  unsigned char value;
 
-  this->robot_device->read_byte_register(DARWIN_MM_CONTROL,&status);
-  status&=0xFD;
-  this->robot_device->write_byte_register(DARWIN_MM_CONTROL,status);
+  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_balance_enabled(void)
+bool CDarwinRobot::mm_is_servo_enabled(unsigned char servo_id)
 {
   unsigned char value;
 
-  this->robot_device->read_byte_register(DARWIN_MM_STATUS,&value);
-  if(value==0x01)
-    return true;
+  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
-    return false;
+    throw CDarwinRobotException(_HERE_,"Invalid robot device");
 }
 
-
 void CDarwinRobot::mm_assign_module(unsigned char servo_id, mm_mode_t mode)
 {
   unsigned char value;
 
-  if(servo_id>MAX_NUM_SERVOS)
-  {
-    // handle exceptions
-    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&=0xF8;
-    value|=(unsigned char)mode;
-  }
-  else// even servo
+  if(this->robot_device!=NULL)
   {
-    value&=0x8F;
-    value|=(((unsigned char)mode)<<4);
+    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);
   }
-  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)
@@ -622,17 +691,21 @@ mm_mode_t CDarwinRobot::mm_get_module(unsigned char servo_id)
 {
   unsigned char value;
 
-  if(servo_id>MAX_NUM_SERVOS)
+  if(this->robot_device!=NULL)
   {
-    // handle exceptions
-    throw CDarwinRobotException(_HERE_,"Invalid servo identifier");
+    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);
   }
-  // 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&0x07);
   else
-    return (mm_mode_t)((value&0x70)>>4);
+    throw CDarwinRobotException(_HERE_,"Invalid robot device");
 }
 
 void CDarwinRobot::mm_load_config(std::string &filename)
@@ -660,34 +733,94 @@ void CDarwinRobot::mm_load_config(std::string &filename)
 std::vector<double> CDarwinRobot::mm_get_servo_angles(void)
 {
   int i=0;
-  short int values[MAX_NUM_SERVOS]; 
+  short int values[MAX_NUM_SERVOS];
   std::vector<double> angles(MAX_NUM_SERVOS);
 
-  this->robot_device->read_registers(DARWIN_MM_SERVO0_CUR_POS_L,(unsigned char *)values,MAX_NUM_SERVOS*2);
+  if(this->robot_device!=NULL)
+  {
+    this->robot_device->read_registers(DARWIN_MM_SERVO0_CUR_POS_L,(unsigned char *)values,MAX_NUM_SERVOS*2);
 
-  for(i=0;i<MAX_NUM_SERVOS;i++)
-    angles[i]=((double)values[i])/128.0;
+    for(i=0;i<MAX_NUM_SERVOS;i++)
+      angles[i]=((double)values[i])/128.0;
 
-  return angles;
+    return angles;
+  }
+  else
+    throw CDarwinRobotException(_HERE_,"Invalid robot device");
 }
 
 double CDarwinRobot::mm_get_servo_angle(unsigned char servo_id)
 {
   unsigned short int value;
   double angle;
-  
-  if(servo_id>MAX_NUM_SERVOS)
+
+  if(this->robot_device!=NULL)
+  {
+    if(servo_id>MAX_NUM_SERVOS)
+    {
+      throw CDarwinRobotException(_HERE_,"Invalid servo identifier");
+    }
+    this->robot_device->read_word_register(DARWIN_MM_SERVO0_CUR_POS_L+servo_id/2,&value);
+    angle=((double)value)/128.0;
+
+    return angle;
+  }
+  else
+    throw CDarwinRobotException(_HERE_,"Invalid robot device");
+}
+
+void CDarwinRobot::mm_set_servo_offset(unsigned char servo_id,double offset)
+{
+  if(offset<-9.0 || offset>9.0)
+    throw CDarwinRobotException(_HERE_,"Desired offset out of range");
+  else
   {
-    / handle exceptions
+    if(servo_id>MAX_NUM_SERVOS)
+      throw CDarwinRobotException(_HERE_,"Invalid servo identifier");
+    else
+    {
+      this->robot_device->write_byte_register(DARWIN_MM_SERVO0_OFFSET+servo_id,(unsigned char)((int)(offset*16.0)));
+      usleep(10000);
+    }
+  }
+}
+
+double CDarwinRobot::mm_get_servo_offset(unsigned char servo_id)
+{
+  unsigned char value;
+  double offset;
+
+  if(servo_id>MAX_NUM_SERVOS)
     throw CDarwinRobotException(_HERE_,"Invalid servo identifier");
+  else
+  {
+    this->robot_device->read_byte_register(DARWIN_MM_SERVO0_OFFSET+servo_id,&value);
+    offset=((double)((signed char)value))/16.0;
+
+    return offset;
   }
-  this->robot_device->read_word_register(DARWIN_MM_SERVO0_CUR_POS_L+servo_id/2,&value);
-  angle=((double)value)/128.0;
+}
 
-  return angle;
+std::vector<double> CDarwinRobot::mm_get_servo_offsets(void)
+{
+  int i=0;
+  signed char values[MAX_NUM_SERVOS];
+  std::vector<double> offsets(MAX_NUM_SERVOS);
+
+  if(this->robot_device!=NULL)
+  {
+    this->robot_device->read_registers(DARWIN_MM_SERVO0_OFFSET,(unsigned char *)values,MAX_NUM_SERVOS);
+
+    for(i=0;i<MAX_NUM_SERVOS;i++)
+      offsets[i]=((double)values[i])/16.0;
+
+    return offsets;
+  }
+  else
+    throw CDarwinRobotException(_HERE_,"Invalid robot device");
 }
 
-void CDarwinRobot::mm_set_balance_gains(double knee,double ankle_pitch,double hip_roll,double ankle_roll)
+/*void CDarwinRobot::mm_set_balance_gains(double knee,double ankle_pitch,double hip_roll,double ankle_roll)
 {
   this->robot_device->write_byte_register(DARWIN_MM_KNEE_GAIN,(unsigned char)(knee*64.0));
   this->robot_device->write_byte_register(DARWIN_MM_ANKLE_PITCH_GAIN,(unsigned char)(ankle_pitch*64.0));
diff --git a/src/darwin_robot.h b/src/darwin_robot.h
index a9ddc75ae75515d7d8ff6c267a7f31cde4e2af52..27bab9794aa7edc05f66513d476d9a1774b54e62 100644
--- a/src/darwin_robot.h
+++ b/src/darwin_robot.h
@@ -45,6 +45,7 @@ class CDarwinRobot
     unsigned char robot_id;
     // motion manager variables
     unsigned char num_servos;
+    unsigned char manager_status;
   public:
     CDarwinRobot(const std::string &name,std::string &bus_id,int bus_speed, unsigned char id);
     // GPIO interface
@@ -77,25 +78,28 @@ class CDarwinRobot
     // motion manager interface
     void mm_set_period(double period_ms);
     double mm_get_period(void);
-    double mm_get_current_period(void);
-    void mm_get_num_servos(int *v1_servos,int *v2_servos);
+    unsigned char mm_get_num_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_servo(unsigned char servo_id);
     void mm_disable_servo(unsigned char servo_id);
     bool mm_is_servo_enabled(unsigned char servo_id);
-    void mm_enable_balance(void);
-    void mm_disable_balance(void);
-    bool mm_is_balance_enabled(void);
     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);
-    void mm_load_config(std::string &filename);
     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
diff --git a/src/examples/darwin_adc_test.cpp b/src/examples/darwin_adc_test.cpp
index 492f4f357546743f891b153d6b6361d9d914201b..08f2c3ae87d6b931fec36efc3c56627cc75b5fba 100644
--- a/src/examples/darwin_adc_test.cpp
+++ b/src/examples/darwin_adc_test.cpp
@@ -14,7 +14,7 @@ int main(int argc, char *argv[])
     CDarwinRobot darwin("Darwin",robot_device,926100,0x02);
     std::cout << "found darwin controller" << std::endl;
     darwin.adc_start();
-    for(i=0;i<1000;i++)
+    for(i=0;i<50;i++)
     {
       std::cout << "Temperature: " << darwin.adc_get_temperature() << std::endl;
       std::cout << "Vref: " << darwin.adc_get_vrefint() << std::endl;