diff --git a/src/CMakeLists.txt b/src/CMakeLists.txt
index dffe47020be3497ece8c2b177a6db832f7a5754e..8e1d04e725b895a4628830e40aa0c1be96075af4 100644
--- a/src/CMakeLists.txt
+++ b/src/CMakeLists.txt
@@ -1,4 +1,4 @@
-SET(DARWIN_FW_PATH /home/shernand/humanoids/darwin_stm32_fw)
+SET(DARWIN_FW_PATH ~/humanoids/darwin_stm32_fw)
 
 ADD_SUBDIRECTORY(xml)
 IF(HAVE_XSD)
@@ -56,6 +56,8 @@ INSTALL(TARGETS darwin_robot
         LIBRARY DESTINATION lib/iridrivers
         ARCHIVE DESTINATION lib/iridrivers)
 INSTALL(FILES ${robot_headers} DESTINATION include/iridrivers)
+INSTALL(FILES ${DARWIN_FW_PATH}/include/darwin_registers.h DESTINATION include/iridrivers)
+INSTALL(FILES ${DARWIN_FW_PATH}/include/action_id.h DESTINATION include/iridrivers)
 IF(KDL_FOUND)
   INSTALL(FILES ${kin_heders} DESTINATION include/iridrivers)
   INSTALL(TARGETS darwin_arm_kinematics
diff --git a/src/darwin_robot.cpp b/src/darwin_robot.cpp
index 7f0c58706705330dc18b19bd6a45c1f94782026c..a2bcbec542b30ed8b99708504da2c04dfecbcd67 100644
--- a/src/darwin_robot.cpp
+++ b/src/darwin_robot.cpp
@@ -855,6 +855,20 @@ unsigned char CDarwinRobot::action_get_current_page(void)
     throw CDarwinRobotException(_HERE_,"Invalid robot device");
 }
 
+unsigned char CDarwinRobot::action_get_current_step(void)
+{
+  unsigned char step_id;
+
+  if(this->robot_device!=NULL)
+  {
+    this->robot_device->read_byte_register(DARWIN_ACTION_CNTRL,&step_id);
+
+    return (step_id>>5);
+  }
+  else
+    throw CDarwinRobotException(_HERE_,"Invalid robot device");
+}
+
 void CDarwinRobot::action_start(void)
 {
   if(this->robot_device!=NULL)
diff --git a/src/darwin_robot.h b/src/darwin_robot.h
index 116487ec93198c4e469b80d52d35a7bd35cafec0..7e08847bd177765cdaff28e6c0592a1262ce6af4 100644
--- a/src/darwin_robot.h
+++ b/src/darwin_robot.h
@@ -109,6 +109,7 @@ class CDarwinRobot
     // motion action interface
     void action_load_page(unsigned char page_id);
     unsigned char action_get_current_page(void);
+    unsigned char action_get_current_step(void);
     void action_start(void);
     void action_stop(void);
     bool action_is_page_running(void);
diff --git a/src/examples/darwin_action_test.cpp b/src/examples/darwin_action_test.cpp
index f9a6e31d07816395b043161099b29c447f93d465..7afacc4de3fc361a5d09bc9862c7db05a53e3036 100644
--- a/src/examples/darwin_action_test.cpp
+++ b/src/examples/darwin_action_test.cpp
@@ -12,7 +12,7 @@ int main(int argc, char *argv[])
   int i=0,num_servos;
 
   try{
-    CDarwinRobot darwin("Darwin",robot_device,926100,0x02);
+    CDarwinRobot darwin("Darwin",robot_device,1000000,0x02);
 
     num_servos=darwin.mm_get_num_servos();
     std::cout << "Found " << num_servos << " servos " << std::endl;
diff --git a/src/examples/darwin_adc_test.cpp b/src/examples/darwin_adc_test.cpp
index 08f2c3ae87d6b931fec36efc3c56627cc75b5fba..7548ce3da8db13c4c1005111be58402c2b901b94 100644
--- a/src/examples/darwin_adc_test.cpp
+++ b/src/examples/darwin_adc_test.cpp
@@ -11,7 +11,7 @@ int main(int argc, char *argv[])
   int i=0;
 
   try{
-    CDarwinRobot darwin("Darwin",robot_device,926100,0x02);
+    CDarwinRobot darwin("Darwin",robot_device,1000000,0x02);
     std::cout << "found darwin controller" << std::endl;
     darwin.adc_start();
     for(i=0;i<50;i++)
diff --git a/src/examples/darwin_gpio_test.cpp b/src/examples/darwin_gpio_test.cpp
index d03307378be43a245ecbf69ae9ea6a299c1c0068..85d6c6dc45be8e6a7a11af72e593a7bdaa532a70 100644
--- a/src/examples/darwin_gpio_test.cpp
+++ b/src/examples/darwin_gpio_test.cpp
@@ -9,7 +9,7 @@ std::string robot_device="A4008atn";
 int main(int argc, char *argv[])
 {
   try{
-    CDarwinRobot darwin("Darwin",robot_device,926100,0x02);
+    CDarwinRobot darwin("Darwin",robot_device,1000000,0x02);
     std::cout << "found darwin controller" << std::endl;
     darwin.gpio_blink_led(LED_TX,1000);
     darwin.gpio_blink_led(LED_RX,2000);
diff --git a/src/examples/darwin_imu_test.cpp b/src/examples/darwin_imu_test.cpp
index 6cc915ec2072c9658075f95ad1ea1661de0212ec..c48e3b4b2bd011d5f5ec63a86ed90726ed151e27 100644
--- a/src/examples/darwin_imu_test.cpp
+++ b/src/examples/darwin_imu_test.cpp
@@ -12,7 +12,7 @@ int main(int argc, char *argv[])
   double accel_x,accel_y,accel_z;
 
   try{
-    CDarwinRobot darwin("Darwin",robot_device,926100,0x02);
+    CDarwinRobot darwin("Darwin",robot_device,1000000,0x02);
     std::cout << "found darwin controller" << std::endl;
     std::cout << "Number of calibration samples: " << darwin.imu_get_cal_samples() << std::endl;
     darwin.imu_start();
diff --git a/src/examples/darwin_manager_test.cpp b/src/examples/darwin_manager_test.cpp
index ca2edb853b8df2ed221220977fa52cd0019a9611..7b3fd220292239dd30c5afb51336febe620dec32 100644
--- a/src/examples/darwin_manager_test.cpp
+++ b/src/examples/darwin_manager_test.cpp
@@ -14,7 +14,7 @@ int main(int argc, char *argv[])
   std::vector<double> angles,speeds,accels,offsets;
 
   try{
-    CDarwinRobot darwin("Darwin",robot_device,926100,0x02);
+    CDarwinRobot darwin("Darwin",robot_device,1000000,0x02);
     darwin.mm_load_config(config_file);
     num_servos=darwin.mm_get_num_servos();
     std::cout << "Found " << num_servos << " servos" << std::endl;
diff --git a/src/examples/darwin_walking_test.cpp b/src/examples/darwin_walking_test.cpp
index 23faa1869decd3dfe4541ed532ee2b1bcc90465d..5f474f4da29ce122926fdd0907b9b50e3ed1efff 100644
--- a/src/examples/darwin_walking_test.cpp
+++ b/src/examples/darwin_walking_test.cpp
@@ -13,7 +13,7 @@ int main(int argc, char *argv[])
   std::vector<double> angles;
 
   try{
-    CDarwinRobot darwin("Darwin",robot_device,926100,0x02);
+    CDarwinRobot darwin("Darwin",robot_device,1000000,0x02);
     num_servos=darwin.mm_get_num_servos();
     std::cout << "Found " << num_servos << " servos" << std::endl;
     // enable all servos and assign them to the action module