diff --git a/src/CMakeLists.txt b/src/CMakeLists.txt
index a6fbf6e1d0815b9285f6691a3d0e0c056343d09a..30fbad80f9702110f5c43aee0431e6d2e2334c28 100644
--- a/src/CMakeLists.txt
+++ b/src/CMakeLists.txt
@@ -13,7 +13,7 @@ INCLUDE_DIRECTORIES(${iriutils_INCLUDE_DIR})
 INCLUDE_DIRECTORIES(${comm_INCLUDE_DIR})
 INCLUDE_DIRECTORIES(${dynamixel_INCLUDE_DIR})
 INCLUDE_DIRECTORIES(${bbb_gpio_INCLUDE_DIR})
-INCLUDE_DIRECTORIES(~/bioloid_robot/stm32_code/include/)
+INCLUDE_DIRECTORIES(~/humanoids/bioloid_stm32_fw/include/)
 # create the shared library
 ADD_LIBRARY(bioloid_robot SHARED ${sources})
 # link necessary libraries
diff --git a/src/bioloid_robot.cpp b/src/bioloid_robot.cpp
index e6fff49bf5c9cf8afff3961e47515e1b06d47a81..e459005be7ab43f7d26c17e9f8a2c123f1034ae2 100644
--- a/src/bioloid_robot.cpp
+++ b/src/bioloid_robot.cpp
@@ -875,7 +875,7 @@ bool CBioloidRobot::action_is_page_running(void)
 }
 
 /* walking interface */
-void walk_set_x_offset(double offset_m)
+void CBioloidRobot::walk_set_x_offset(double offset_m)
 {
   unsigned char offset;
 
@@ -888,7 +888,7 @@ void walk_set_x_offset(double offset_m)
     throw CBioloidRobotException(_HERE_,"Invalid robot device");
 }
 
-double walk_get_x_offset(void)
+double CBioloidRobot::walk_get_x_offset(void)
 {
   unsigned char offset;
 
@@ -901,7 +901,7 @@ double walk_get_x_offset(void)
     throw CBioloidRobotException(_HERE_,"Invalid robot device");
 }
 
-void walk_set_y_offset(double offset_m)
+void CBioloidRobot::walk_set_y_offset(double offset_m)
 {
   unsigned char offset;
 
@@ -914,7 +914,7 @@ void walk_set_y_offset(double offset_m)
     throw CBioloidRobotException(_HERE_,"Invalid robot device");
 }
 
-double walk_get_y_offset(void)
+double CBioloidRobot::walk_get_y_offset(void)
 {
   unsigned char offset;
 
@@ -927,7 +927,7 @@ double walk_get_y_offset(void)
     throw CBioloidRobotException(_HERE_,"Invalid robot device");
 }
 
-void walk_set_z_offset(double offset_m)
+void CBioloidRobot::walk_set_z_offset(double offset_m)
 {
   unsigned char offset;
 
@@ -940,7 +940,7 @@ void walk_set_z_offset(double offset_m)
     throw CBioloidRobotException(_HERE_,"Invalid robot device");
 }
 
-unsigned int walk_get_z_offset(void)
+double CBioloidRobot::walk_get_z_offset(void)
 {
   unsigned char offset;
 
@@ -953,7 +953,7 @@ unsigned int walk_get_z_offset(void)
     throw CBioloidRobotException(_HERE_,"Invalid robot device");
 }
 
-void walk_set_roll_offset(double offset_rad)
+void CBioloidRobot::walk_set_roll_offset(double offset_rad)
 {
   unsigned char offset;
 
@@ -966,7 +966,7 @@ void walk_set_roll_offset(double offset_rad)
     throw CBioloidRobotException(_HERE_,"Invalid robot device");
 }
 
-double walk_get_roll_offset(void)
+double CBioloidRobot::walk_get_roll_offset(void)
 {
   unsigned char offset;
 
@@ -979,7 +979,7 @@ double walk_get_roll_offset(void)
     throw CBioloidRobotException(_HERE_,"Invalid robot device");
 }
 
-void walk_set_pitch_offset(double offset_rad)
+void CBioloidRobot::walk_set_pitch_offset(double offset_rad)
 {
   unsigned char offset;
 
@@ -992,7 +992,7 @@ void walk_set_pitch_offset(double offset_rad)
     throw CBioloidRobotException(_HERE_,"Invalid robot device");
 }
 
-double walk_get_pitch_offset(void)
+double CBioloidRobot::walk_get_pitch_offset(void)
 {
   unsigned char offset;
 
@@ -1005,7 +1005,7 @@ double walk_get_pitch_offset(void)
     throw CBioloidRobotException(_HERE_,"Invalid robot device");
 }
 
-void walk_set_yaw_offset(double offset_rad)
+void CBioloidRobot::walk_set_yaw_offset(double offset_rad)
 {
   unsigned char offset;
 
@@ -1018,7 +1018,7 @@ void walk_set_yaw_offset(double offset_rad)
     throw CBioloidRobotException(_HERE_,"Invalid robot device");
 }
 
-double walk_get_yaw_offset(void)
+double CBioloidRobot::walk_get_yaw_offset(void)
 {
   unsigned char offset;
 
@@ -1031,7 +1031,7 @@ double walk_get_yaw_offset(void)
     throw CBioloidRobotException(_HERE_,"Invalid robot device");
 }
 
-void walk_set_hip_picth_offset(double offset_rad)
+void CBioloidRobot::walk_set_hip_picth_offset(double offset_rad)
 {
   unsigned short int offset;
 
@@ -1044,7 +1044,7 @@ void walk_set_hip_picth_offset(double offset_rad)
     throw CBioloidRobotException(_HERE_,"Invalid robot device");
 }
 
-double walk_get_hip_pitch_offset(void)
+double CBioloidRobot::walk_get_hip_pitch_offset(void)
 {
   unsigned short int offset;
 
@@ -1057,7 +1057,7 @@ double walk_get_hip_pitch_offset(void)
     throw CBioloidRobotException(_HERE_,"Invalid robot device");
 }
 
-void walk_set_period(unsigned int period_s)
+void CBioloidRobot::walk_set_period(double period_s)
 {
   unsigned short int period;
 
@@ -1070,7 +1070,7 @@ void walk_set_period(unsigned int period_s)
     throw CBioloidRobotException(_HERE_,"Invalid robot device");
 }
 
-unsigned int walk_get_period(void)
+double CBioloidRobot::walk_get_period(void)
 {
   unsigned short int period;
 
@@ -1083,7 +1083,7 @@ unsigned int walk_get_period(void)
     throw CBioloidRobotException(_HERE_,"Invalid robot device");
 }
 
-void walk_set_ssp_dsp_ratio(double ratio)
+void CBioloidRobot::walk_set_ssp_dsp_ratio(double ratio)
 {
   unsigned char ratio_int;
 
@@ -1096,7 +1096,7 @@ void walk_set_ssp_dsp_ratio(double ratio)
     throw CBioloidRobotException(_HERE_,"Invalid robot device");
 }
 
-double walk_get_ssp_dsp_ratio(void)
+double CBioloidRobot::walk_get_ssp_dsp_ratio(void)
 {
   unsigned char ratio_int;
 
@@ -1109,7 +1109,7 @@ double walk_get_ssp_dsp_ratio(void)
     throw CBioloidRobotException(_HERE_,"Invalid robot device");
 }
 
-void walk_set_fwd_bwd_ratio(double ratio)
+void CBioloidRobot::walk_set_fwd_bwd_ratio(double ratio)
 {
   unsigned char ratio_int;
 
@@ -1122,7 +1122,7 @@ void walk_set_fwd_bwd_ratio(double ratio)
     throw CBioloidRobotException(_HERE_,"Invalid robot device");
 }
 
-double walk_get_fwd_bwd_ratio(void)
+double CBioloidRobot::walk_get_fwd_bwd_ratio(void)
 {
   unsigned char ratio_int;
 
@@ -1135,7 +1135,7 @@ double walk_get_fwd_bwd_ratio(void)
     throw CBioloidRobotException(_HERE_,"Invalid robot device");
 }
 
-void walk_set_foot_height(double height_m)
+void CBioloidRobot::walk_set_foot_height(double height_m)
 {
   unsigned char height;
 
@@ -1148,7 +1148,7 @@ void walk_set_foot_height(double height_m)
     throw CBioloidRobotException(_HERE_,"Invalid robot device");
 }
 
-double walk_get_foot_height(void)
+double CBioloidRobot::walk_get_foot_height(void)
 {
   unsigned char height;
 
@@ -1161,7 +1161,7 @@ double walk_get_foot_height(void)
     throw CBioloidRobotException(_HERE_,"Invalid robot device");
 }
 
-void walk_set_left_right_swing(double swing_m)
+void CBioloidRobot::walk_set_left_right_swing(double swing_m)
 {
   unsigned char swing;
 
@@ -1174,20 +1174,20 @@ void walk_set_left_right_swing(double swing_m)
     throw CBioloidRobotException(_HERE_,"Invalid robot device");
 }
 
-double walk_get_left_right_swing(void)
+double CBioloidRobot::walk_get_left_right_swing(void)
 {
   unsigned char swing;
 
   if(this->robot_device!=NULL)
   {
     this->robot_device->read_byte_register(BIOLOID_WALK_SWING_RIGHT_LEFT,&swing);
-    return ((double)wsing)/1000.0;
+    return ((double)swing)/1000.0;
   }
   else
     throw CBioloidRobotException(_HERE_,"Invalid robot device");
 }
 
-void walk_set_top_down_swing(double swing_m)
+void CBioloidRobot::walk_set_top_down_swing(double swing_m)
 {
   unsigned char swing;
 
@@ -1200,20 +1200,20 @@ void walk_set_top_down_swing(double swing_m)
     throw CBioloidRobotException(_HERE_,"Invalid robot device");
 }
 
-double walk_get_top_down_swing(void)
+double CBioloidRobot::walk_get_top_down_swing(void)
 {
   unsigned char swing;
 
   if(this->robot_device!=NULL)
   {
     this->robot_device->read_byte_register(BIOLOID_WALK_SWING_TOP_DOWN,&swing);
-    return ((double)wsing)/1000.0;
+    return ((double)swing)/1000.0;
   }
   else
     throw CBioloidRobotException(_HERE_,"Invalid robot device");
 }
 
-void walk_set_pelvis_offset(double offset_rad)
+void CBioloidRobot::walk_set_pelvis_offset(double offset_rad)
 {
   unsigned char offset;
 
@@ -1226,7 +1226,7 @@ void walk_set_pelvis_offset(double offset_rad)
     throw CBioloidRobotException(_HERE_,"Invalid robot device");
 }
 
-double walk_get_pelvis_offset(void)
+double CBioloidRobot::walk_get_pelvis_offset(void)
 {
   unsigned char offset;
 
@@ -1239,7 +1239,7 @@ double walk_get_pelvis_offset(void)
     throw CBioloidRobotException(_HERE_,"Invalid robot device");
 }
 
-void walk_set_arm_swing_gain(double gain)
+void CBioloidRobot::walk_set_arm_swing_gain(double gain)
 {
   unsigned char gain_int;
 
@@ -1252,7 +1252,7 @@ void walk_set_arm_swing_gain(double gain)
     throw CBioloidRobotException(_HERE_,"Invalid robot device");
 }
 
-double walk_get_arm_swing_gain(void)
+double CBioloidRobot::walk_get_arm_swing_gain(void)
 {
   unsigned char gain_int;
 
@@ -1265,7 +1265,7 @@ double walk_get_arm_swing_gain(void)
     throw CBioloidRobotException(_HERE_,"Invalid robot device");
 }
 
-void walk_set_trans_speed(double speed_m_s)
+void CBioloidRobot::walk_set_trans_speed(double speed_m_s)
 {
   unsigned char speed;
 
@@ -1278,7 +1278,7 @@ void walk_set_trans_speed(double speed_m_s)
     throw CBioloidRobotException(_HERE_,"Invalid robot device");
 }
 
-double walk_get_trans_speed(void)
+double CBioloidRobot::walk_get_trans_speed(void)
 {
   unsigned char speed;
 
@@ -1291,7 +1291,7 @@ double walk_get_trans_speed(void)
     throw CBioloidRobotException(_HERE_,"Invalid robot device");
 }
 
-void walk_set_rot_speed(double speed_rad_s)
+void CBioloidRobot::walk_set_rot_speed(double speed_rad_s)
 {
   unsigned char speed;
 
@@ -1304,7 +1304,7 @@ void walk_set_rot_speed(double speed_rad_s)
     throw CBioloidRobotException(_HERE_,"Invalid robot device");
 }
 
-double walk_get_rot_speed(void)
+double CBioloidRobot::walk_get_rot_speed(void)
 {
   unsigned char speed;
 
@@ -1317,7 +1317,7 @@ double walk_get_rot_speed(void)
     throw CBioloidRobotException(_HERE_,"Invalid robot device");
 }
 
-void walk_start(void)
+void CBioloidRobot::walk_start(void)
 {
   if(this->robot_device!=NULL)
   {
@@ -1328,7 +1328,7 @@ void walk_start(void)
     throw CBioloidRobotException(_HERE_,"Invalid robot device");
 }
 
-void walk_stop(void)
+void CBioloidRobot::walk_stop(void)
 {
   if(this->robot_device!=NULL)
   {
@@ -1339,7 +1339,7 @@ void walk_stop(void)
     throw CBioloidRobotException(_HERE_,"Invalid robot device");
 }
 
-bool is_walking(void)
+bool CBioloidRobot::is_walking(void)
 {
   unsigned char value;
 
@@ -1355,7 +1355,7 @@ bool is_walking(void)
     throw CBioloidRobotException(_HERE_,"Invalid robot device");
 }
 
-void walk_set_x_step(double step_m)
+void CBioloidRobot::walk_set_x_step(double step_m)
 {
   unsigned char step;
 
@@ -1368,7 +1368,7 @@ void walk_set_x_step(double step_m)
     throw CBioloidRobotException(_HERE_,"Invalid robot device");
 }
 
-double walk_get_x_step(void)
+double CBioloidRobot::walk_get_x_step(void)
 {
   unsigned char step;
 
@@ -1381,7 +1381,7 @@ double walk_get_x_step(void)
     throw CBioloidRobotException(_HERE_,"Invalid robot device");
 }
 
-void walk_set_y_step(double step_m)
+void CBioloidRobot::walk_set_y_step(double step_m)
 {
   unsigned char step;
 
@@ -1394,7 +1394,7 @@ void walk_set_y_step(double step_m)
     throw CBioloidRobotException(_HERE_,"Invalid robot device");
 }
 
-double walk_get_y_step(void)
+double CBioloidRobot::walk_get_y_step(void)
 {
   unsigned char step;
 
@@ -1407,7 +1407,7 @@ double walk_get_y_step(void)
     throw CBioloidRobotException(_HERE_,"Invalid robot device");
 }
 
-void walk_set_turn_step(double step_rad)
+void CBioloidRobot::walk_set_turn_step(double step_rad)
 {
   unsigned char step;
 
@@ -1420,7 +1420,7 @@ void walk_set_turn_step(double step_rad)
     throw CBioloidRobotException(_HERE_,"Invalid robot device");
 }
 
-double walk_get_turn_step(void)
+double CBioloidRobot::walk_get_turn_step(void)
 {
   unsigned char step;
 
diff --git a/src/bioloid_robot.h b/src/bioloid_robot.h
index bf61301aa34e508292e5983cc61e478ac017bbb3..d7633f0bb0cf3789034256e4b7eace55d481a9b2 100644
--- a/src/bioloid_robot.h
+++ b/src/bioloid_robot.h
@@ -11,6 +11,7 @@
 #include "eventserver.h"
 
 #define MAX_NUM_SERVOS        31
+#define PI                    3.14159
 
 /* available motion modules */
 typedef enum {BIOLOID_MM_NONE=0x00,BIOLOID_MM_ACTION=0x01,BIOLOID_MM_WALKING=0x02,BIOLOID_MM_JOINTS=0x03} mm_mode_t;
@@ -173,48 +174,48 @@ class CBioloidRobot
     bool action_is_int_enabled(void);
     bool action_is_page_running(void);
     /* walking interface */
-    void walk_set_x_offset(unsigned int offset_mm);
-    unsigned int walk_get_x_offset(void);
-    void walk_set_y_offset(unsigned int offset_mm);
-    unsigned int walk_get_y_offset(void);
-    void walk_set_z_offset(unsigned int offset_mm);
-    unsigned int walk_get_z_offset(void);
-    void walk_set_roll_offset(double offset_deg);
+    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_deg);
+    void walk_set_pitch_offset(double offset_rad);
     double walk_get_pitch_offset(void);
-    void walk_set_yaw_offset(double offset_deg);
+    void walk_set_yaw_offset(double offset_rad);
     double walk_get_yaw_offset(void);
-    void walk_set_hip_picth_offset(double offset_deg);
+    void walk_set_hip_picth_offset(double offset_rad);
     double walk_get_hip_pitch_offset(void);
-    void walk_set_period(unsigned int period_ms);
-    unsigned int walk_get_period(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(unsigned int height_mm);
-    unsigned int walk_get_foot_height(void);
-    void walk_set_left_right_swing(unsigned int swing_mm);
-    unsigned int walk_get_left_right_swing(void);
-    void walk_set_top_down_swing(unsigned int swing_mm);
-    unsigned int walk_get_top_down_swing(void);
-    void walk_set_pelvis_offset(double offset_deg);
+    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_mm_s);
+    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(unsigned int step_mm);
-    unsigned int walk_get_x_step(void);
-    void walk_set_y_step(unsigned int step_mm);
-    unsigned int walk_get_y_step(void);
-    void walk_set_turn_step(double step_deg);
+    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);
     ~CBioloidRobot();
 };