diff --git a/src/CMakeLists.txt b/src/CMakeLists.txt
index a9586aa6e302241aa89a21882beceaf964b7ce6e..a6fbf6e1d0815b9285f6691a3d0e0c056343d09a 100644
--- a/src/CMakeLists.txt
+++ b/src/CMakeLists.txt
@@ -29,3 +29,4 @@ INSTALL(TARGETS bioloid_robot
 INSTALL(FILES ${headers} DESTINATION include/iridrivers)
 INSTALL(FILES ../Findbioloid_robot.cmake DESTINATION ${CMAKE_ROOT}/Modules/)
 ADD_SUBDIRECTORY(examples)
+ADD_SUBDIRECTORY(tools)
diff --git a/src/bioloid_robot.cpp b/src/bioloid_robot.cpp
index 49fd95cda2c09dca36d3826cd2b8a7d5f01f5821..f7fffba0ab0d296c510e8b08cd6945064c32f769 100644
--- a/src/bioloid_robot.cpp
+++ b/src/bioloid_robot.cpp
@@ -707,7 +707,7 @@ double CBioloidRobot::mm_get_servo_angle(unsigned char servo_id)
     throw CBioloidRobotException(_HERE_,"Invalid robot device");
 }
 
-void CBioloidRobot::mm_set_offset(unsigned char servo_id,double offset)
+void CBioloidRobot::mm_set_servo_offset(unsigned char servo_id,double offset)
 {
   if(offset<-9.0 || offset>9.0)
     throw CBioloidRobotException(_HERE_,"Desired offset out of range");
@@ -723,7 +723,7 @@ void CBioloidRobot::mm_set_offset(unsigned char servo_id,double offset)
   }
 }
 
-double CBioloidRobot::mm_get_offset(unsigned char servo_id)
+double CBioloidRobot::mm_get_servo_offset(unsigned char servo_id)
 {
   unsigned char value;
   double offset;
@@ -739,6 +739,24 @@ double CBioloidRobot::mm_get_offset(unsigned char servo_id)
   } 
 }
 
+std::vector<double> CBioloidRobot::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(BIOLOID_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 CBioloidRobotException(_HERE_,"Invalid robot device");
+}
 // motion action interface
 void CBioloidRobot::action_load_page(unsigned char page_id)
 {
diff --git a/src/bioloid_robot.h b/src/bioloid_robot.h
index 2e997f2c50af6f7ada1efec13548e3e343c51730..320ee929462e79180db72fe5c61416d460741825 100644
--- a/src/bioloid_robot.h
+++ b/src/bioloid_robot.h
@@ -158,8 +158,9 @@ class CBioloidRobot
     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_offset(unsigned char servo_id,double offset);
-    double mm_get_offset(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);
     // motion action interface
     void action_load_page(unsigned char page_id);
     unsigned char action_get_current_page(void);
diff --git a/src/examples/CMakeLists.txt b/src/examples/CMakeLists.txt
index a5b05ecf70bdf338d9e45ee0194eb1b324a398f3..79349548b02bf8837d0eea20b582f3ef050f4cd8 100644
--- a/src/examples/CMakeLists.txt
+++ b/src/examples/CMakeLists.txt
@@ -1,4 +1,24 @@
 # create an example application
-ADD_EXECUTABLE(bioloid_robot_test bioloid_robot_test.cpp)
+ADD_EXECUTABLE(bioloid_robot_gpio_test bioloid_robot_gpio_test.cpp)
 # link necessary libraries
-TARGET_LINK_LIBRARIES(bioloid_robot_test bioloid_robot)
+TARGET_LINK_LIBRARIES(bioloid_robot_gpio_test bioloid_robot)
+
+# create an example application
+ADD_EXECUTABLE(bioloid_robot_adc_test bioloid_robot_adc_test.cpp)
+# link necessary libraries
+TARGET_LINK_LIBRARIES(bioloid_robot_adc_test bioloid_robot)
+
+# create an example application
+ADD_EXECUTABLE(bioloid_robot_zigbee_test bioloid_robot_zigbee_test.cpp)
+# link necessary libraries
+TARGET_LINK_LIBRARIES(bioloid_robot_zigbee_test bioloid_robot)
+
+# create an example application
+ADD_EXECUTABLE(bioloid_robot_mm_status_test bioloid_robot_mm_status_test.cpp)
+# link necessary libraries
+TARGET_LINK_LIBRARIES(bioloid_robot_mm_status_test bioloid_robot)
+
+# create an example application
+ADD_EXECUTABLE(bioloid_robot_mm_action_test bioloid_robot_mm_action_test.cpp)
+# link necessary libraries
+TARGET_LINK_LIBRARIES(bioloid_robot_mm_action_test bioloid_robot)
diff --git a/src/examples/bioloid_robot_adc_test.cpp b/src/examples/bioloid_robot_adc_test.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..9ad5b1849b20909ab757b09a521eb1c59dbb3460
--- /dev/null
+++ b/src/examples/bioloid_robot_adc_test.cpp
@@ -0,0 +1,31 @@
+#include "bioloid_robot.h"
+#include "bioloid_robot_exceptions.h"
+#include "action_id.h"
+
+#include <iostream>
+#include <stdint.h>
+
+std::string robot_device="/dev/ttyO1";
+
+int main(int argc, char *argv[])
+{
+  unsigned int i=0;
+  
+  try{
+    CBioloidRobot tina("Tina",robot_device,921600,0x02,GPIO2,25);
+    for(i=0;i<100;i++)
+    {
+      std::cout << "temperature: " << tina.adc_get_temperature() << std::endl;
+      std::cout << "Vref_int: " << tina.adc_get_vrefint() << std::endl;
+      std::cout << "Channel 1: " << tina.adc_get_value(ADC_CH1) << std::endl;
+      std::cout << "Channel 2: " << tina.adc_get_value(ADC_CH2) << std::endl;
+      std::cout << "Channel 3: " << tina.adc_get_value(ADC_CH3) << std::endl;
+      std::cout << "Channel 4: " << tina.adc_get_value(ADC_CH4) << std::endl;
+      std::cout << "Channel 5: " << tina.adc_get_value(ADC_CH5) << std::endl;
+      std::cout << "Channel 6: " << tina.adc_get_value(ADC_CH6) << std::endl;
+//      usleep(100000);
+    }
+  }catch(CException &e){
+    std::cout << e.what() << std::endl;
+  }
+}
diff --git a/src/examples/bioloid_robot_gpio_test.cpp b/src/examples/bioloid_robot_gpio_test.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..c1fca46ed7e2389064274f28a90b77c9069a46a9
--- /dev/null
+++ b/src/examples/bioloid_robot_gpio_test.cpp
@@ -0,0 +1,71 @@
+#include "bioloid_robot.h"
+#include "bioloid_robot_exceptions.h"
+#include "action_id.h"
+
+#include <iostream>
+#include <stdint.h>
+
+std::string robot_device="/dev/ttyO1";
+
+class CCallbacks
+{
+  public:
+    CCallbacks(){};
+    void user_pb_callback(CBioloidRobot *robot)
+    {
+      std::cout << "user push button pressed" << std::endl;
+    }
+    void reset_pb_callback(CBioloidRobot *robot)
+    {
+      std::cout << "reset push button pressed" << std::endl;
+    }
+    void start_pb_callback(CBioloidRobot *robot)
+    {
+      std::cout << "start push button pressed" << std::endl;
+    }
+    void mode_pb_callback(CBioloidRobot *robot)
+    {
+      std::cout << "mode push button pressed" << std::endl;
+    }
+    ~CCallbacks(){};
+};
+
+int main(int argc, char *argv[])
+{
+  unsigned int i=0;
+  CCallbacks callbacks;
+  
+  try{
+    CBioloidRobot tina("Tina",robot_device,921600,0x02,GPIO2,25);
+    tina.assign_button_callback(USER_PB,&callbacks,&CCallbacks::user_pb_callback);
+    tina.assign_button_callback(RESET_PB,&callbacks,&CCallbacks::reset_pb_callback);
+    tina.assign_button_callback(START_PB,&callbacks,&CCallbacks::start_pb_callback);
+    tina.assign_button_callback(MODE_PB,&callbacks,&CCallbacks::mode_pb_callback);
+    for(i=0;i<4;i++)
+    {
+      tina.toggle_led(RXD_LED);
+      tina.toggle_led(TXD_LED);
+      tina.toggle_led(USER1_LED);
+      tina.toggle_led(USER2_LED);
+      sleep(1);
+    }
+    tina.clear_led(RXD_LED);
+    tina.clear_led(TXD_LED);
+    tina.clear_led(USER1_LED);
+    tina.clear_led(USER2_LED);
+
+    tina.blink_led(TXD_LED,1000);
+    tina.blink_led(RXD_LED,2000);
+    tina.blink_led(USER1_LED,1000);
+    tina.blink_led(USER2_LED,2000);
+    sleep(10);
+
+    tina.clear_led(RXD_LED);
+    tina.clear_led(TXD_LED);
+    tina.clear_led(USER1_LED);
+    tina.clear_led(USER2_LED);
+    sleep(10);
+  }catch(CException &e){
+    std::cout << e.what() << std::endl;
+  }
+}
diff --git a/src/examples/bioloid_robot_mm_action_test.cpp b/src/examples/bioloid_robot_mm_action_test.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..cdd34ece2f269ed65dbe48b1d60c5616ae90b9f1
--- /dev/null
+++ b/src/examples/bioloid_robot_mm_action_test.cpp
@@ -0,0 +1,67 @@
+#include "bioloid_robot.h"
+#include "bioloid_robot_exceptions.h"
+#include "action_id.h"
+
+#include <iostream>
+#include <stdint.h>
+
+std::string robot_device="/dev/ttyO1";
+
+int main(int argc, char *argv[])
+{
+  std::vector<double> angles;  
+  unsigned int i=0,j=0;
+  
+  try{
+    CBioloidRobot tina("Tina",robot_device,921600,0x02,GPIO2,25);
+    std::cout << "Number of servos: " << (int)tina.mm_get_num_servos() << std::endl;
+    for(i=0;i<=MAX_NUM_SERVOS;i++)
+    {
+      tina.mm_enable_servo(i);
+      tina.mm_assign_module(i,BIOLOID_MM_ACTION);
+      if(tina.mm_is_servo_enabled(i))
+        std::cout << "Servo " << i << " is enabled";
+      else
+        std::cout << "Servo " << i << " is disabled";
+      std::cout << " and assigned to module " << tina.mm_get_module(i) << std::endl; 
+    }
+    tina.mm_start();
+    tina.mm_enable_power();
+    sleep(1);
+    tina.action_load_page(WALK_READY);
+    tina.action_start();
+    while(tina.action_is_page_running())
+    {
+      // get the servos positions
+      angles=tina.mm_get_servo_angles();
+      for(j=0;j<=MAX_NUM_SERVOS;j++)
+        std::cout << "Servo " << j << " angle: " << angles[j] << std::endl;
+      usleep(100000);
+    }
+    tina.action_load_page(SIT_DOWN);
+    tina.action_start();
+    while(tina.action_is_page_running())
+    {
+      // get the servos positions
+      angles=tina.mm_get_servo_angles();
+      for(j=0;j<=MAX_NUM_SERVOS;j++)
+        std::cout << "Servo " << j << " angle: " << angles[j] << std::endl;
+      usleep(100000);
+    }
+    sleep(1);
+    tina.action_load_page(STAND_UP);
+    tina.action_start();
+    while(tina.action_is_page_running())
+    {
+      // get the servos positions
+      angles=tina.mm_get_servo_angles();
+      for(j=0;j<=MAX_NUM_SERVOS;j++)
+        std::cout << "Servo " << j << " angle: " << angles[j] << std::endl;
+      usleep(100000);
+    }
+    tina.mm_disable_power();
+    tina.mm_stop();
+  }catch(CException &e){
+    std::cout << e.what() << std::endl;
+  }
+}
diff --git a/src/examples/bioloid_robot_mm_status_test.cpp b/src/examples/bioloid_robot_mm_status_test.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..5f73d5d031fcfc8675ee0a4d0e5ea82768811cc7
--- /dev/null
+++ b/src/examples/bioloid_robot_mm_status_test.cpp
@@ -0,0 +1,55 @@
+#include "bioloid_robot.h"
+#include "bioloid_robot_exceptions.h"
+#include "action_id.h"
+
+#include <iostream>
+#include <stdint.h>
+
+std::string robot_device="/dev/ttyO1";
+
+int main(int argc, char *argv[])
+{
+  std::vector<double> angles;  
+  unsigned int i=0,j=0;
+  
+  try{
+    CBioloidRobot tina("Tina",robot_device,921600,0x02,GPIO2,25);
+    std::cout << "Number of servos: " << (int)tina.mm_get_num_servos() << std::endl;
+    tina.mm_set_period(7.8);
+    std::cout << "Motion manager period: " << tina.mm_get_period() << std::endl;
+    // check the state of all servos
+    for(i=0;i<=MAX_NUM_SERVOS;i++)
+    {
+      if(tina.mm_is_servo_enabled(i))
+        std::cout << "Servo " << i << " is enabled";
+      else
+        std::cout << "Servo " << i << " is disabled";
+      std::cout << " and assigned to module " << tina.mm_get_module(i) << std::endl; 
+    }
+    // enable all servos and assign them to the ACTION MODULE
+    for(i=0;i<=MAX_NUM_SERVOS;i++)
+    {
+      tina.mm_disable_servo(i);
+      tina.mm_assign_module(i,BIOLOID_MM_NONE);
+      if(tina.mm_is_servo_enabled(i))
+        std::cout << "Servo " << i << " is enabled";
+      else
+        std::cout << "Servo " << i << " is disabled";
+      std::cout << " and assigned to module " << tina.mm_get_module(i) << std::endl; 
+    }
+    tina.mm_start();
+    tina.mm_enable_power();
+    for(i=0;i<1000;i++)
+    {
+      // get the servos positions
+      angles=tina.mm_get_servo_angles();
+      for(j=0;j<=MAX_NUM_SERVOS;j++)
+        std::cout << "Servo " << j << " angle: " << angles[j] << std::endl;
+      usleep(100000);
+    }
+    tina.mm_disable_power();
+    tina.mm_stop();
+  }catch(CException &e){
+    std::cout << e.what() << std::endl;
+  }
+}
diff --git a/src/examples/bioloid_robot_test.cpp b/src/examples/bioloid_robot_test.cpp
deleted file mode 100644
index 81a73374b5ac557b31d7a55c0aa3eba2b68fdb80..0000000000000000000000000000000000000000
--- a/src/examples/bioloid_robot_test.cpp
+++ /dev/null
@@ -1,266 +0,0 @@
-#include "bioloid_robot.h"
-#include "bioloid_robot_exceptions.h"
-#include "action_id.h"
-
-#include <iostream>
-#include <stdint.h>
-
-std::string robot_device="/dev/ttyO1";
-
-class CCallbacks
-{
-  public:
-    CCallbacks(){};
-    void user_pb_callback(CBioloidRobot *robot)
-    {
-      std::cout << "user push button pressed" << std::endl;
-    }
-    void reset_pb_callback(CBioloidRobot *robot)
-    {
-      std::cout << "reset push button pressed" << std::endl;
-    }
-    void start_pb_callback(CBioloidRobot *robot)
-    {
-      std::cout << "start push button pressed" << std::endl;
-    }
-    void mode_pb_callback(CBioloidRobot *robot)
-    {
-      std::cout << "mode push button pressed" << std::endl;
-    }
-    ~CCallbacks(){};
-};
-
-int main(int argc, char *argv[])
-{
-  std::vector<double> angles;  
-  unsigned int i=0,j=0;
-  CCallbacks callbacks;
-  unsigned char key;
-  double offset=0.0;
-  
-  try{
-    CBioloidRobot tina("Tina",robot_device,921600,0x02,GPIO2,25);
-/*    tina.assign_button_callback(USER_PB,&callbacks,&CCallbacks::user_pb_callback);
-    tina.assign_button_callback(RESET_PB,&callbacks,&CCallbacks::reset_pb_callback);
-    tina.assign_button_callback(START_PB,&callbacks,&CCallbacks::start_pb_callback);
-    tina.assign_button_callback(MODE_PB,&callbacks,&CCallbacks::mode_pb_callback);
-    for(i=0;i<4;i++)
-    {
-      tina.toggle_led(RXD_LED);
-      tina.toggle_led(TXD_LED);
-      tina.toggle_led(USER1_LED);
-      tina.toggle_led(USER2_LED);
-      sleep(1);
-    }
-    tina.clear_led(RXD_LED);
-    tina.clear_led(TXD_LED);
-    tina.clear_led(USER1_LED);
-    tina.clear_led(USER2_LED);
-
-    tina.blink_led(TXD_LED,1000);
-    tina.blink_led(RXD_LED,2000);
-    tina.blink_led(USER1_LED,1000);
-    tina.blink_led(USER2_LED,2000);
-    sleep(10);
-
-    tina.clear_led(RXD_LED);
-    tina.clear_led(TXD_LED);
-    tina.clear_led(USER1_LED);
-    tina.clear_led(USER2_LED);
-    sleep(10);*/
-/*    for(i=0;i<100;i++)
-    {
-      std::cout << "temperature: " << tina.adc_get_temperature() << std::endl;
-      std::cout << "Vref_int: " << tina.adc_get_vrefint() << std::endl;
-      std::cout << "Channel 1: " << tina.adc_get_value(ADC_CH1) << std::endl;
-      std::cout << "Channel 2: " << tina.adc_get_value(ADC_CH2) << std::endl;
-      std::cout << "Channel 3: " << tina.adc_get_value(ADC_CH3) << std::endl;
-      std::cout << "Channel 4: " << tina.adc_get_value(ADC_CH4) << std::endl;
-      std::cout << "Channel 5: " << tina.adc_get_value(ADC_CH5) << std::endl;
-      std::cout << "Channel 6: " << tina.adc_get_value(ADC_CH6) << std::endl;
-      usleep(100000);
-    }*/
-/*    tina.zigbee_enable_power();
-    sleep(3);
-    tina.zigbee_disable_power();*/
-/*    std::cout << "Number of servos: " << (int)tina.mm_get_num_servos() << std::endl;
-    tina.mm_set_period(7.8);
-    std::cout << "Motion manager period: " << tina.mm_get_period() << std::endl;
-    // check the state of all servos
-    for(i=0;i<=MAX_NUM_SERVOS;i++)
-    {
-      if(tina.mm_is_servo_enabled(i))
-        std::cout << "Servo " << i << " is enabled";
-      else
-        std::cout << "Servo " << i << " is disabled";
-      std::cout << " and assigned to module " << tina.mm_get_module(i) << std::endl; 
-    }
-    // enable all servos and assign them to the ACTION MODULE
-    for(i=0;i<=MAX_NUM_SERVOS;i++)
-    {
-      tina.mm_disable_servo(i);
-      tina.mm_assign_module(i,BIOLOID_MM_NONE);
-      if(tina.mm_is_servo_enabled(i))
-        std::cout << "Servo " << i << " is enabled";
-      else
-        std::cout << "Servo " << i << " is disabled";
-      std::cout << " and assigned to module " << tina.mm_get_module(i) << std::endl; 
-    }
-    tina.mm_start();
-    tina.mm_enable_power();
-    for(i=0;i<1000;i++)
-    {
-      // get the servos positions
-      angles=tina.mm_get_servo_angles();
-      for(j=0;j<=MAX_NUM_SERVOS;j++)
-        std::cout << "Servo " << j << " angle: " << angles[j] << std::endl;
-      usleep(100000);
-    }
-    tina.mm_disable_power();
-    tina.mm_stop();*/
-/*    std::cout << "Number of servos: " << (int)tina.mm_get_num_servos() << std::endl;
-    for(i=0;i<=MAX_NUM_SERVOS;i++)
-    {
-      tina.mm_enable_servo(i);
-      tina.mm_assign_module(i,BIOLOID_MM_ACTION);
-      if(tina.mm_is_servo_enabled(i))
-        std::cout << "Servo " << i << " is enabled";
-      else
-        std::cout << "Servo " << i << " is disabled";
-      std::cout << " and assigned to module " << tina.mm_get_module(i) << std::endl; 
-    }
-    tina.mm_start();
-//    tina.mm_disable_balance();
-    tina.mm_enable_balance();
-    tina.mm_enable_power();
-    sleep(1);
-    tina.action_load_page(WALK_READY);
-    tina.action_start();
-    while(tina.action_is_page_running())
-    {
-      // get the servos positions
-      angles=tina.mm_get_servo_angles();
-      for(j=0;j<=MAX_NUM_SERVOS;j++)
-        std::cout << "Servo " << j << " angle: " << angles[j] << std::endl;
-      usleep(100000);
-    }
-    tina.action_load_page(SIT_DOWN);
-    tina.action_start();
-    while(tina.action_is_page_running())
-    {
-      // get the servos positions
-      angles=tina.mm_get_servo_angles();
-      for(j=0;j<=MAX_NUM_SERVOS;j++)
-        std::cout << "Servo " << j << " angle: " << angles[j] << std::endl;
-      usleep(100000);
-    }
-    sleep(1);
-    tina.action_load_page(STAND_UP);
-    tina.action_start();
-    while(tina.action_is_page_running())
-    {
-      // get the servos positions
-      angles=tina.mm_get_servo_angles();
-      for(j=0;j<=MAX_NUM_SERVOS;j++)
-        std::cout << "Servo " << j << " angle: " << angles[j] << std::endl;
-      usleep(100000);
-    }
-    for(i=0;i<100;i++)
-    {
-      // get the servos positions
-      angles=tina.mm_get_servo_angles();
-//      for(j=0;j<=MAX_NUM_SERVOS;j++)
-//        std::cout << "Servo " << j << " angle: " << angles[j] << std::endl;
-      std::cout << "Servo " << 13 << " angle: " << angles[13] << std::endl;
-      std::cout << "Channel 1: " << tina.adc_get_value(ADC_CH1) << std::endl;
-      std::cout << "Channel 2: " << tina.adc_get_value(ADC_CH2) << std::endl;
-      usleep(100000);
-    }
-    tina.mm_disable_power();
-    tina.mm_stop();*/
-    //tina.mm_set_period(7.8);
-/*    std::cout << "Motion manager period: " << tina.mm_get_period() << std::endl;
-    std::cout << "Number of servos: " << (int)tina.mm_get_num_servos() << std::endl;
-    for(i=0;i<=MAX_NUM_SERVOS;i++)
-    {
-      tina.mm_enable_servo(i);
-      tina.mm_assign_module(i,BIOLOID_MM_ACTION);
-      if(tina.mm_is_servo_enabled(i))
-        std::cout << "Servo " << i << " is enabled";
-      else
-        std::cout << "Servo " << i << " is disabled";
-      std::cout << " and assigned to module " << tina.mm_get_module(i) << std::endl; 
-    }
-    tina.mm_start();
-    //tina.mm_enable_balance();
-    tina.mm_enable_power();
-    tina.action_load_page(WALK_READY);
-    tina.action_start();
-    while(tina.action_is_page_running())
-      usleep(100000);    
-    sleep(2);
-    tina.action_load_page(RT_S_R);
-    tina.action_start();
-    for(i=0;i<100;i++)
-    {
-      std::cout << "Current page: " << (int)tina.action_get_current_page() << std::endl;
-      usleep(100000);
-    }
-    tina.action_stop();
-    while(tina.action_is_page_running())
-      usleep(100000);    
-    sleep(2);
-    tina.mm_disable_power();
-    tina.mm_stop();*/
-    tina.mm_set_offset(1,0.0);
-    tina.mm_set_offset(2,0.0);
-    tina.mm_set_offset(8,0.0);
-    tina.mm_set_offset(9,0.0);
-    tina.mm_set_offset(10,0.0);//5.25
-    tina.mm_set_offset(11,0.0);//1.75
-    tina.mm_set_offset(12,0.0);//4.0
-    tina.mm_set_offset(13,0.0);
-    tina.mm_set_offset(14,0.0);
-    tina.mm_set_offset(15,0.0);//1.75
-    tina.mm_set_offset(16,0.0);//-4
-    tina.mm_set_offset(17,0.0);
-    tina.mm_set_offset(18,0.0);
-    for(i=0;i<=MAX_NUM_SERVOS;i++)
-    {
-      std::cout << "Servo " << i << " offset: " << tina.mm_get_offset(i) << std::endl;
-    }
-    std::cout << "Motion manager period: " << tina.mm_get_period() << std::endl;
-    std::cout << "Number of servos: " << (int)tina.mm_get_num_servos() << std::endl;
-    for(i=0;i<=MAX_NUM_SERVOS;i++)
-    {
-      tina.mm_enable_servo(i);
-      tina.mm_assign_module(i,BIOLOID_MM_ACTION);
-      if(tina.mm_is_servo_enabled(i))
-        std::cout << "Servo " << i << " is enabled";
-      else
-        std::cout << "Servo " << i << " is disabled";
-      std::cout << " and assigned to module " << tina.mm_get_module(i) << std::endl; 
-    }
-    tina.mm_start();
-    //tina.mm_enable_balance();
-    tina.mm_enable_power();
-    tina.action_load_page(WALK_READY);
-    tina.action_start();
-    while(tina.action_is_page_running())
-      usleep(100000);    
-    do{
-      std::cin >> key;
-      if(key=='+')
-        offset+=0.5;
-      else if(key=='-')
-        offset-=0.5;
-      std::cout << offset << std::endl;
-      tina.mm_set_offset(1,offset);
-    }while(key!='q');
-    sleep(10);
-    tina.mm_disable_power();
-    tina.mm_stop();
-  }catch(CException &e){
-    std::cout << e.what() << std::endl;
-  }
-}
diff --git a/src/examples/bioloid_robot_zigbee_test.cpp b/src/examples/bioloid_robot_zigbee_test.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..225a4016019d3d10c9bf644036930825db329e0e
--- /dev/null
+++ b/src/examples/bioloid_robot_zigbee_test.cpp
@@ -0,0 +1,20 @@
+#include "bioloid_robot.h"
+#include "bioloid_robot_exceptions.h"
+#include "action_id.h"
+
+#include <iostream>
+#include <stdint.h>
+
+std::string robot_device="/dev/ttyO1";
+
+int main(int argc, char *argv[])
+{
+  try{
+    CBioloidRobot tina("Tina",robot_device,921600,0x02,GPIO2,25);
+    tina.zigbee_enable_power();
+    sleep(3);
+    tina.zigbee_disable_power();
+  }catch(CException &e){
+    std::cout << e.what() << std::endl;
+  }
+}
diff --git a/src/tools/CMakeLists.txt b/src/tools/CMakeLists.txt
new file mode 100644
index 0000000000000000000000000000000000000000..7b118e2bac0548b5000add2114afef5935b41957
--- /dev/null
+++ b/src/tools/CMakeLists.txt
@@ -0,0 +1,12 @@
+Set(CURSES_NEED_NCURSES TRUE)
+
+FIND_PACKAGE(Curses REQUIRED)
+
+INCLUDE_DIRECTORIES(${CURSES_INCLUDE_DIR})
+
+# create an example application
+ADD_EXECUTABLE(bioloid_offset bioloid_offset.cpp)
+# link necessary libraries
+TARGET_LINK_LIBRARIES(bioloid_offset bioloid_robot)
+TARGET_LINK_LIBRARIES(bioloid_offset ${CURSES_LIBRARIES})
+
diff --git a/src/tools/bioloid_offset.cpp b/src/tools/bioloid_offset.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..0952484a08c1e7d3686c65cc313b94ce28e512e2
--- /dev/null
+++ b/src/tools/bioloid_offset.cpp
@@ -0,0 +1,303 @@
+#include "bioloid_robot.h"
+#include "bioloid_robot_exceptions.h"
+#include "action_id.h"
+#include "mutex.h"
+#include "threadserver.h"
+#include "eventserver.h"
+
+#include <ncurses.h>
+#include <stdint.h>
+#include <math.h>
+#include <signal.h>
+#include <iostream>
+
+std::string robot_device="/dev/ttyO1";
+
+const int num_key_cmd=7;
+const int max_key_cmd_length=30;
+const unsigned char key_cmds[num_key_cmd][max_key_cmd_length]={"UP - select upper servo",
+                                                               "DOWN - select lower servo",
+                                                               "+ - increment servo offset",
+                                                               "- - decrement servo offset",
+                                                               "i - increment delta",
+                                                               "d - decrement delta",
+                                                               "q - quit"};
+
+const unsigned char servo_info[]=" Servo         Angle          Offset";
+const unsigned char current_inc[]="current angle increment";
+
+// bioloid global attributes
+int current_servo_sel=1;
+double increment=0.0625;
+CBioloidRobot *tina=NULL;
+// window global attributes
+CMutex window_access;
+std::string resize_event_id;
+std::string finish_thread_event_id;
+
+int get_cmd_per_line(int col)
+{
+  return col/max_key_cmd_length;
+}
+
+int get_num_cmd_lines(int col)
+{
+  int cmd_per_line;
+
+  cmd_per_line=get_cmd_per_line(col);
+  if(cmd_per_line==0)
+    return -1;
+  else
+    return ceil((double)num_key_cmd/(double)cmd_per_line);
+}
+
+int show_key_cmds(int row,int col)
+{
+  int i,j,num_lines,cmd_id=0;
+
+  // cout number of lines to fit all the commands
+  if((num_lines=get_num_cmd_lines(col))<0)
+    return -1;
+  for(i=0;i<col;i++)
+    mvprintw(row-(num_lines+1),i,"-");
+  for(i=row-num_lines;i<row;i++)
+  {
+    for(j=0;j<get_cmd_per_line(col);j++)
+    {
+      if(cmd_id<num_key_cmd)
+      {
+        mvprintw(i,j*max_key_cmd_length,"%s",key_cmds[cmd_id]);
+        cmd_id++;
+      }
+    }
+  }
+
+  return 0;
+}
+
+int show_servo_info(int row,int col)
+{
+  int i,num_servos,num_cmd_lines,num_servo_lines,servo_id;
+  std::vector<double> angles;
+  std::vector<double> offsets;
+  static int start_show=1;
+
+  num_servos=tina->mm_get_num_servos(); 
+  if((num_cmd_lines=get_num_cmd_lines(col))<0)
+    return -1;
+  num_servo_lines=row-num_cmd_lines-3;
+  // get servo angles and offsets
+  angles=tina->mm_get_servo_angles();
+  offsets=tina->mm_get_servo_offsets();
+  attron(A_BOLD);
+  mvprintw(0,4,"%s",servo_info);
+  attroff(A_BOLD);
+  if(num_servos>num_servo_lines)
+  {
+    if(start_show>current_servo_sel)
+      start_show=current_servo_sel;
+    else
+    {
+      if(start_show+num_servo_lines<=current_servo_sel)
+        start_show=current_servo_sel-num_servo_lines+1;
+    }  
+    if(start_show>1)
+      mvprintw(1,1,"*");
+    if(num_servos-start_show>=num_servo_lines)
+      mvprintw(num_servo_lines,1,"*");
+  }
+  else
+    start_show=1;
+  for(i=0;i<std::min(num_servo_lines,num_servos);i++)
+  {
+    servo_id=start_show+i;
+    if(servo_id==current_servo_sel)
+    {
+      attron(A_BOLD);
+      attron(COLOR_PAIR(1));
+    }
+    else
+    {
+      attroff(COLOR_PAIR(1));
+      attroff(A_BOLD);
+    }
+    mvprintw(i+1,4,"Servo %d",servo_id);
+    mvprintw(i+1,16,"%f",angles[servo_id]);
+    mvprintw(i+1,32,"%f",offsets[servo_id]);
+  }
+
+  attroff(COLOR_PAIR(1));
+  attroff(A_BOLD);
+
+  // show current angle increment
+  mvprintw(0,col-sizeof(current_inc),"%s",current_inc);
+  mvprintw(1,col-2*sizeof(current_inc)/3,"%f",increment);
+
+  return 0;
+}
+
+void *resize_thread(void *param)
+{
+  CEventServer *event_server=CEventServer::instance();
+  std::list<std::string> events;
+  int event_id,row,col;
+  bool end=false;
+  
+  events.push_back(resize_event_id);
+  events.push_back(finish_thread_event_id);
+  while(!end)
+  {
+    event_id=event_server->wait_first(events);
+    if(event_id==0)// resize event
+    {
+      try{
+        window_access.enter();
+        endwin();
+        refresh();
+        clear();
+        getmaxyx(stdscr,row,col);
+        show_key_cmds(row,col);    
+        show_servo_info(row,col);
+        refresh();
+        window_access.exit();
+      }catch(CException &e){
+        window_access.exit();
+      }
+    }
+    else if(event_id==1)
+      end=true;
+  }
+
+  pthread_exit(NULL);
+}
+
+void do_resize(int dummy)
+{
+  CEventServer *event_server=CEventServer::instance();
+
+  if(!event_server->event_is_set(resize_event_id))
+    event_server->set_event(resize_event_id);
+}
+
+int main(int argc, char *argv[])
+{
+  CEventServer *event_server=CEventServer::instance();
+  CThreadServer *thread_server=CThreadServer::instance();
+  std::string resize_thread_id;
+  std::vector<double> offsets;
+  unsigned int i=0,row,col;
+  unsigned char key;
+  
+  try{
+    resize_event_id="window_resize_event";
+    event_server->create_event(resize_event_id);
+    finish_thread_event_id="finish_thread_event";
+    event_server->create_event(finish_thread_event_id);
+    resize_thread_id="resize_thread";
+    thread_server->create_thread(resize_thread_id);
+    thread_server->attach_thread(resize_thread_id,resize_thread,NULL);
+    thread_server->start_thread(resize_thread_id);
+    // assign a function to handle window resize events
+    signal(SIGWINCH, do_resize);
+    // initialize the robot object
+    tina=new CBioloidRobot("Tina",robot_device,921600,0x02,GPIO2,25);
+    for(i=0;i<=MAX_NUM_SERVOS;i++)
+    {
+      tina->mm_enable_servo(i);
+      tina->mm_assign_module(i,BIOLOID_MM_ACTION);
+    }
+    tina->mm_start();
+    tina->mm_enable_power();
+    tina->action_load_page(WALK_READY);
+    tina->action_start();
+    while(tina->action_is_page_running())
+      usleep(100000);
+    offsets=tina->mm_get_servo_offsets();
+    // initialize the screen
+    initscr();
+    start_color();
+    init_pair(1, COLOR_GREEN, COLOR_BLACK);
+    keypad(stdscr,true);
+    do{
+      getmaxyx(stdscr,row,col);
+      show_key_cmds(row,col);    
+      show_servo_info(row,col);
+      refresh();
+      key=getch();
+      if(key=='+')
+      {
+        offsets[current_servo_sel]+=increment;
+        if(offsets[current_servo_sel]>7.5)
+          offsets[current_servo_sel]=7.5;
+        else if(offsets[current_servo_sel]<-7.5)
+          offsets[current_servo_sel]=-7.5;
+        tina->mm_set_servo_offset(current_servo_sel,offsets[current_servo_sel]);
+      }
+      else if(key=='-')
+      {
+        offsets[current_servo_sel]-=increment;
+        if(offsets[current_servo_sel]>7.5)
+          offsets[current_servo_sel]=7.5;
+        else if(offsets[current_servo_sel]<-7.5)
+          offsets[current_servo_sel]=-7.5;
+        tina->mm_set_servo_offset(current_servo_sel,offsets[current_servo_sel]);
+      }
+      else if(key=='i')
+      {
+        increment+=0.0625;
+        if(increment>2.0)
+          increment=2.0;
+        else if(increment<0.0625)
+          increment=0.0625;
+      }
+      else if(key=='d')
+      {
+        increment-=0.0625;
+        if(increment>2.0)
+          increment=2.0;
+        else if(increment<0.0625)
+          increment=0.0625;
+      }
+      else if(key==3)
+      {
+        if(current_servo_sel>1)
+          current_servo_sel--;
+      }
+      else if(key==2)
+      {
+        if(current_servo_sel<tina->mm_get_num_servos())
+          current_servo_sel++;
+      }
+      clear();
+    }while(key!='q');
+
+    endwin(); 
+    event_server->set_event(finish_thread_event_id);
+    thread_server->end_thread(resize_thread_id);
+    thread_server->detach_thread(resize_thread_id);
+    thread_server->delete_thread(resize_thread_id);
+    event_server->delete_event(resize_event_id);
+    event_server->delete_event(finish_thread_event_id);
+    tina->action_load_page(SIT_DOWN);
+    tina->action_start();
+    while(tina->action_is_page_running())
+      usleep(100000);
+    tina->mm_disable_power();
+    tina->mm_stop();
+    delete tina;
+
+    return 0;
+  }catch(CException &e){
+    endwin(); 
+    event_server->set_event(finish_thread_event_id);
+    thread_server->end_thread(resize_thread_id);
+    thread_server->detach_thread(resize_thread_id);
+    thread_server->delete_thread(resize_thread_id);
+    event_server->delete_event(resize_event_id);
+    event_server->delete_event(finish_thread_event_id);
+    tina->mm_disable_power();
+    tina->mm_stop();
+    if(tina!=NULL)
+      delete tina;
+  }
+}