diff --git a/include/action.h b/include/action.h
index 693ccc167a525e600e8c1f30b103b23f593bf394..97315bede8b2236116dcbf23006d7760f7a061e7 100644
--- a/include/action.h
+++ b/include/action.h
@@ -12,8 +12,8 @@ extern int64_t action_current_angles[PAGE_MAX_NUM_SERVOS];
 extern uint8_t action_slope[PAGE_MAX_NUM_SERVOS];
 
 // public functions
-void action_init(uint16_t period);
-inline void action_set_period(uint16_t period);
+void action_init(uint16_t period_us);
+inline void action_set_period(uint16_t period_us);
 inline uint16_t action_get_period(void);
 uint8_t action_load_page(uint8_t page_id);
 void action_start_page(void);
diff --git a/include/motion_manager.h b/include/motion_manager.h
index e6a1616b2776b95096e80f7e77504eba4cf237dc..2885584c858f9b7864c86b5c6640fb6bc2c75bdf 100644
--- a/include/motion_manager.h
+++ b/include/motion_manager.h
@@ -76,7 +76,7 @@ inline uint8_t manager_is_enabled(void);
 void manager_enable_balance(void);
 void manager_disable_balance(void);
 inline uint8_t manager_get_num_servos(void);
-inline void manager_set_module(uint8_t servo_id,TModules module);
+void manager_set_module(uint8_t servo_id,TModules module);
 inline TModules manager_get_module(uint8_t servo_id);
 inline void manager_enable_servo(uint8_t servo_id);
 inline void manager_disable_servo(uint8_t servo_id);
diff --git a/src/action.c b/src/action.c
index 72f2d0ef97d396dd24e12ece3e2a4f7bf16732f9..a6e06989d829322b805c6359393269a6b1af3303 100644
--- a/src/action.c
+++ b/src/action.c
@@ -33,6 +33,7 @@ int64_t action_pause_time;// fixed point 48|16 format
 int64_t action_current_time;// fixed point 48|16 format
 int64_t action_section_time;// fixed point 48|16 format
 int64_t action_period;
+int16_t action_period_us;
 
 // private functions
 void action_load_next_step(void)
@@ -219,7 +220,6 @@ void action_load_next_step(void)
 void action_finish(void)
 {
   // set the internal state machine to the idle state
-  action_load_next_step();
   action_end=0x00;
   // clear the status falg for the action module
   ram_data[BIOLOID_ACTION_CNTRL]&=(~ACTION_STATUS);
@@ -233,7 +233,7 @@ void action_finish(void)
 }
 
 // public functions
-void action_init(uint16_t period)
+void action_init(uint16_t period_us)
 {
   unsigned char i;
 
@@ -266,17 +266,19 @@ void action_init(uint16_t period)
   action_current_time=0;// fixed point 48|16 format
   action_section_time=0;// fixed point 48|16 format
 
-  action_period=period;
+  action_period=(period_us<<16)/1000000;
+  action_period_us=period_us;
 }
 
-inline void action_set_period(uint16_t period)
+inline void action_set_period(uint16_t period_us)
 {
-  action_period=period;
+  action_period=(period_us<<16)/1000000;
+  action_period_us=period_us;
 }
 
 inline uint16_t action_get_period(void)
 {
-  return action_period;
+  return action_period_us;
 }
 
 uint8_t action_load_page(uint8_t page_id)
diff --git a/src/motion_manager.c b/src/motion_manager.c
index 4370b172836180863b4663148d4a8c9f85a293f6..8a172e0def975f19b1e07078d6fc3b250fead857 100644
--- a/src/motion_manager.c
+++ b/src/motion_manager.c
@@ -105,15 +105,17 @@ void manager_get_target_position(void)
 
 void manager_balance(void)
 {
-  adc_ch_t fb_ch=gyro_get_fb_adc_channel(),lr_ch=gyro_get_lr_adc_channel();
+  adc_ch_t fb_ch,lr_ch;
   int16_t knee_gain,ankle_roll_gain,ankle_pitch_gain,hip_roll_gain;
   uint16_t fb_offset,lr_offset;
   int16_t fb_value,lr_value;
 
   if(manager_balance_enabled==0x01)// balance is enabled
   {
+    fb_ch=gyro_get_fb_adc_channel();
+    lr_ch=gyro_get_lr_adc_channel();
     // get the balance gains
-    knee_gain=ram_data[BIOLOID_MM_BAL_KNEE_GAIN_L]+(ram_data[BIOLOID_MM_BAL_KNEE_GAIN_H]*256);
+    knee_gain=ram_data[BIOLOID_MM_BAL_KNEE_GAIN_L]+(ram_data[BIOLOID_MM_BAL_KNEE_GAIN_H]<<8);
     ankle_roll_gain=ram_data[BIOLOID_MM_BAL_ANKLE_ROLL_GAIN_L]+(ram_data[BIOLOID_MM_BAL_ANKLE_ROLL_GAIN_H]<<8);
     ankle_pitch_gain=ram_data[BIOLOID_MM_BAL_ANKLE_PITCH_GAIN_L]+(ram_data[BIOLOID_MM_BAL_ANKLE_PITCH_GAIN_H]<<8);
     hip_roll_gain=ram_data[BIOLOID_MM_BAL_HIP_ROLL_GAIN_L]+(ram_data[BIOLOID_MM_BAL_HIP_ROLL_GAIN_H]<<8);
@@ -123,14 +125,14 @@ void manager_balance(void)
     fb_value=adc_get_channel_raw(fb_ch)-fb_offset;
     lr_value=adc_get_channel_raw(lr_ch)-lr_offset;
     // compensate the servo angle values
-    manager_balance_offset[R_KNEE]=(((fb_value*manager_servos[R_KNEE].max_angle)/manager_servos[R_KNEE].encoder_resolution)*knee_gain)/65536;
-    manager_balance_offset[R_ANKLE_PITCH]=(((fb_value*manager_servos[R_ANKLE_PITCH].max_angle)/manager_servos[R_ANKLE_PITCH].encoder_resolution)*ankle_pitch_gain)/65536;    
-    manager_balance_offset[L_KNEE]=-(((fb_value*manager_servos[L_KNEE].max_angle)/manager_servos[L_KNEE].encoder_resolution)*knee_gain)/65536;    
-    manager_balance_offset[L_ANKLE_PITCH]=-(((fb_value*manager_servos[L_ANKLE_PITCH].max_angle)/manager_servos[L_ANKLE_PITCH].encoder_resolution)*ankle_pitch_gain)/65536;
-    manager_balance_offset[R_HIP_ROLL]=(((lr_value*manager_servos[R_HIP_ROLL].max_angle)/manager_servos[R_HIP_ROLL].encoder_resolution)*hip_roll_gain)/65536;
-    manager_balance_offset[L_HIP_ROLL]=(((lr_value*manager_servos[L_HIP_ROLL].max_angle)/manager_servos[L_HIP_ROLL].encoder_resolution)*hip_roll_gain)/65536;
-    manager_balance_offset[R_ANKLE_ROLL]=-(((lr_value*manager_servos[R_ANKLE_ROLL].max_angle)/manager_servos[R_ANKLE_ROLL].encoder_resolution)*ankle_roll_gain)/65536;
-    manager_balance_offset[L_ANKLE_ROLL]=-(((lr_value*manager_servos[L_ANKLE_ROLL].max_angle)/manager_servos[L_ANKLE_ROLL].encoder_resolution)*ankle_roll_gain)/65536;
+    manager_balance_offset[R_KNEE]=(((fb_value*manager_servos[R_KNEE].max_angle)/manager_servos[R_KNEE].encoder_resolution)*knee_gain)>>16;
+    manager_balance_offset[R_ANKLE_PITCH]=(((fb_value*manager_servos[R_ANKLE_PITCH].max_angle)/manager_servos[R_ANKLE_PITCH].encoder_resolution)*ankle_pitch_gain)>>16;    
+    manager_balance_offset[L_KNEE]=-(((fb_value*manager_servos[L_KNEE].max_angle)/manager_servos[L_KNEE].encoder_resolution)*knee_gain)>>16;    
+    manager_balance_offset[L_ANKLE_PITCH]=-(((fb_value*manager_servos[L_ANKLE_PITCH].max_angle)/manager_servos[L_ANKLE_PITCH].encoder_resolution)*ankle_pitch_gain)>>16;
+    manager_balance_offset[R_HIP_ROLL]=(((lr_value*manager_servos[R_HIP_ROLL].max_angle)/manager_servos[R_HIP_ROLL].encoder_resolution)*hip_roll_gain)>>16;
+    manager_balance_offset[L_HIP_ROLL]=(((lr_value*manager_servos[L_HIP_ROLL].max_angle)/manager_servos[L_HIP_ROLL].encoder_resolution)*hip_roll_gain)>>16;
+    manager_balance_offset[R_ANKLE_ROLL]=-(((lr_value*manager_servos[R_ANKLE_ROLL].max_angle)/manager_servos[R_ANKLE_ROLL].encoder_resolution)*ankle_roll_gain)>>16;
+    manager_balance_offset[L_ANKLE_ROLL]=-(((lr_value*manager_servos[L_ANKLE_ROLL].max_angle)/manager_servos[L_ANKLE_ROLL].encoder_resolution)*ankle_roll_gain)>>16;
   }
 }
 
@@ -184,7 +186,7 @@ void manager_init(uint16_t period_us)
   dyn_master_scan(&bioloid_dyn_master_servos,&num,servo_ids); 
   ram_data[BIOLOID_MM_NUM_SERVOS]=num;
   manager_num_servos=0;
-/*  for(i=0;i<MANAGER_MAX_NUM_SERVOS;i++)
+  for(i=0;i<MANAGER_MAX_NUM_SERVOS;i++)
   {
     if(i==servo_ids[manager_num_servos])
     {
@@ -289,7 +291,7 @@ void manager_init(uint16_t period_us)
       manager_servos[i].ccw_angle_limit=0;
     }
   }
-  bioloid_dyn_master_servos_disable_power();*/
+  bioloid_dyn_master_servos_disable_power();
 
   /* configure timer */
   ENABLE_MANAGER_TIMER_CLK;
@@ -298,27 +300,27 @@ void manager_init(uint16_t period_us)
   MANAGER_TIM_Handle.Init.Prescaler = 84;
   MANAGER_TIM_Handle.Init.ClockDivision = TIM_CLOCKDIVISION_DIV1;
   MANAGER_TIM_Handle.Init.CounterMode = TIM_COUNTERMODE_UP;
-//  HAL_NVIC_SetPriority(MANAGER_TIMER_IRQn, 2, 1);
-//  HAL_NVIC_EnableIRQ(MANAGER_TIMER_IRQn);
+  HAL_NVIC_SetPriority(MANAGER_TIMER_IRQn, 2, 1);
+  HAL_NVIC_EnableIRQ(MANAGER_TIMER_IRQn);
   /* use the internal clock */
-//  sClockSourceConfig.ClockSource = TIM_CLOCKSOURCE_INTERNAL;
-//  HAL_TIM_ConfigClockSource(&MANAGER_TIM_Handle, &sClockSourceConfig);
-//  HAL_TIM_OC_Init(&MANAGER_TIM_Handle);
+  sClockSourceConfig.ClockSource = TIM_CLOCKSOURCE_INTERNAL;
+  HAL_TIM_ConfigClockSource(&MANAGER_TIM_Handle, &sClockSourceConfig);
+  HAL_TIM_OC_Init(&MANAGER_TIM_Handle);
   /* disable master/slave mode */
-//  sMasterConfig.MasterOutputTrigger = TIM_TRGO_RESET;
-//  sMasterConfig.MasterSlaveMode = TIM_MASTERSLAVEMODE_DISABLE;
-//  HAL_TIMEx_MasterConfigSynchronization(&MANAGER_TIM_Handle, &sMasterConfig);
+  sMasterConfig.MasterOutputTrigger = TIM_TRGO_RESET;
+  sMasterConfig.MasterSlaveMode = TIM_MASTERSLAVEMODE_DISABLE;
+  HAL_TIMEx_MasterConfigSynchronization(&MANAGER_TIM_Handle, &sMasterConfig);
   /* configure ouptut counter channel 4 */
-//  manager_motion_period=(period_us*1000000)>>16;
-//  manager_motion_period_us=period_us;
+  manager_motion_period=(period_us<<16)/1000000;
+  manager_motion_period_us=period_us;
 
   /* initialize balance parameters */
-//  for(i=0;i<MANAGER_MAX_NUM_SERVOS;i++)
-//    manager_balance_offset[i]=0;
-//  manager_balance_enabled=0x00;
+  for(i=0;i<MANAGER_MAX_NUM_SERVOS;i++)
+    manager_balance_offset[i]=0;
+  manager_balance_enabled=0x00;
 
   /* initialize action module */
-//  action_init(period_us);
+  action_init(period_us);
 }
 
 uint16_t manager_get_period(void)