diff --git a/src/motion_manager.c b/src/motion_manager.c
index 703a7c81dd905b7fdbb6472a91c058efae3ed82f..c461bfaaddf2a4d3f0d41e0f72e08f8432cfa590 100644
--- a/src/motion_manager.c
+++ b/src/motion_manager.c
@@ -1,7 +1,6 @@
 #include "motion_manager.h"
 #include "dynamixel_master_uart_dma.h"
 #include "dyn_servos.h"
-#include "gpio.h"
 #include "ram.h"
 #include "action.h"
 #include "stm32_time.h"
@@ -15,28 +14,33 @@
 __IO uint16_t manager_motion_period;
 uint8_t manager_num_servos;
 TServoInfo manager_servos[MANAGER_MAX_NUM_SERVOS];
+// current angles used for all motion modules
+int64_t current_angles[MANAGER_MAX_NUM_SERVOS];
+// balance values and configuration
+int64_t balance_offset[MANAGER_MAX_NUM_SERVOS];
+uint8_t balance_enabled;
+// package variables
+uint8_t servo_ids[MANAGER_MAX_NUM_SERVOS];
+TWriteData write_data[MANAGER_MAX_NUM_SERVOS];
+uint8_t length[MANAGER_MAX_NUM_SERVOS];
+uint8_t address[MANAGER_MAX_NUM_SERVOS];
 
 // private functions
 void manager_send_motion_command(void)
 {
-  uint8_t servo_ids[MANAGER_MAX_NUM_SERVOS];
-  uint8_t data[256];
   uint8_t i,num=0;
-  
+
   for(i=0;i<MANAGER_MAX_NUM_SERVOS;i++)
   {
-    if(manager_servos[i].enabled)
+    if(manager_servos[i].enabled && manager_servos[i].model!=0x0000)
     {
       servo_ids[num]=manager_servos[i].id;
-      data[num*4]=action_slope[i];
-      data[num*4+1]=action_slope[i];
-      data[num*4+2]=manager_servos[i].current_value%256;
-      data[num*4+3]=manager_servos[i].current_value/256;
+      write_data[num].data_addr=(uint8_t *)&(manager_servos[i].current_value);
       num++;
     }
   }
   if(num>0)
-    dyn_master_sync_write(num,servo_ids,P_CW_COMPLIANCE_SLOPE,4,data);
+    dyn_master_sync_write(num,servo_ids,P_GOAL_POSITION_L,2,write_data);
 }
 
 inline uint16_t manager_angle_to_value(uint8_t servo_id,int16_t angle)
@@ -67,7 +71,7 @@ void manager_get_current_position(void)
 
   for(i=0;i<MANAGER_MAX_NUM_SERVOS;i++)
   {
-    if(!manager_servos[i].enabled)// servo is enabled
+    if(!manager_servos[i].enabled)// servo is not enabled
     {
       dyn_master_read_word(manager_servos[i].id,P_PRESENT_POSITION_L,&manager_servos[i].current_value);
       manager_servos[i].current_angle=manager_value_to_angle(i,manager_servos[i].current_value);
@@ -87,7 +91,7 @@ void manager_get_target_position(void)
     {
       if(manager_servos[i].module==MM_ACTION)
       {
-        manager_servos[i].current_angle=((action_current_angles[i])>>9);
+        manager_servos[i].current_angle=((current_angles[i]+balance_offset[i])>>9);
         //>>16 from the action codification, <<7 from the manager codification
         manager_servos[i].current_value=manager_angle_to_value(i,manager_servos[i].current_angle);
       }
@@ -97,17 +101,6 @@ void manager_get_target_position(void)
 
 void manager_balance(void)
 {
-  int16_t x_error=0.0;
-  int16_t y_error=0.0;
-
-  manager_servos[10].current_value+=(y_error*ram_data[BIOLOID_MM_PELVIS_ROLL_GAIN_L]*11)/2500;
-  manager_servos[9].current_value+=(y_error*ram_data[BIOLOID_MM_PELVIS_ROLL_GAIN_R]*11)/2500;
-  manager_servos[18].current_value-=(y_error*ram_data[BIOLOID_MM_ANKLE_ROLL_GAIN_L]*11)/2500;
-  manager_servos[17].current_value-=(y_error*ram_data[BIOLOID_MM_ANKLE_ROLL_GAIN_R]*11)/2500;
-  manager_servos[16].current_value-=(x_error*ram_data[BIOLOID_MM_ANKLE_PITCH_GAIN_L]*11)/2500;
-  manager_servos[15].current_value+=(x_error*ram_data[BIOLOID_MM_ANKLE_PITCH_GAIN_R]*11)/2500;
-  manager_servos[14].current_value-=(x_error*ram_data[BIOLOID_MM_KNEE_GAIN_L]*11)/2500;
-  manager_servos[13].current_value+=(x_error*ram_data[BIOLOID_MM_KNEE_GAIN_R]*11)/2500;
 }
 
 // interrupt handlers
@@ -129,7 +122,7 @@ void MOTION_TIMER_IRQHandler(void)
     // get the target angles from all modules
     manager_get_target_position();
     // balance the robot
-//    manager_balance();
+    manager_balance();
     // send the motion commands to the servos
     manager_send_motion_command();
     // get the disabled servos position
@@ -144,14 +137,14 @@ void manager_init(uint16_t period_us)
   TIM_OCInitTypeDef  TIM_OCInitStructure;
   NVIC_InitTypeDef NVIC_InitStructure;
   uint8_t servo_ids[MANAGER_MAX_NUM_SERVOS];
+  uint16_t model,value;
   uint8_t i,num=0;
-  uint16_t model;
 
   RCC_APB1PeriphClockCmd(MOTION_TIMER_CLK,ENABLE);
 
   // enable power to the servos
   dyn_master_enable_power();
-  delay_ms(300);
+  delay_ms(1000);
 
   // detect the servos connected 
   dyn_master_scan(&num,servo_ids); 
@@ -236,8 +229,13 @@ void manager_init(uint16_t period_us)
       manager_servos[i].current_angle=manager_value_to_angle(i,manager_servos[i].current_value);
       ram_data[BIOLOID_MM_SERVO0_CUR_POS_L+i*2]=(manager_servos[i].current_angle&0xFF);
       ram_data[BIOLOID_MM_SERVO0_CUR_POS_H+i*2]=(manager_servos[i].current_angle>>8);
+      // read the servo limits
+      dyn_master_read_word(manager_servos[i].id,P_CW_ANGLE_LIMIT_L,&value);
+      manager_servos[i].cw_angle_limit=manager_value_to_angle(i,value);
+      dyn_master_read_word(manager_servos[i].id,P_CCW_ANGLE_LIMIT_L,&value);
+      manager_servos[i].ccw_angle_limit=manager_value_to_angle(i,value);
       // set the action current angles
-      action_current_angles[i]=manager_servos[i].current_angle<<9;
+      current_angles[i]=manager_servos[i].current_angle<<9;
       manager_num_servos++;
     }
     else
@@ -253,6 +251,8 @@ void manager_init(uint16_t period_us)
       manager_servos[i].current_value=0;
       manager_servos[i].current_angle=0;
       manager_servos[i].enabled=0x00;
+      manager_servos[i].cw_angle_limit=0;
+      manager_servos[i].ccw_angle_limit=0;
     }
   }
   dyn_master_disable_power();
@@ -266,21 +266,28 @@ void manager_init(uint16_t period_us)
 
   /* motion timer configuration */
   TIM_TimeBaseStructure.TIM_Period = 0xFFFF;
-  TIM_TimeBaseStructure.TIM_Prescaler = 72;
+  TIM_TimeBaseStructure.TIM_Prescaler = 42;
   TIM_TimeBaseStructure.TIM_ClockDivision = 0;
   TIM_TimeBaseStructure.TIM_CounterMode = TIM_CounterMode_Up;
   TIM_TimeBaseInit(MOTION_TIMER,&TIM_TimeBaseStructure);
-  TIM_SetClockDivision(MOTION_TIMER,TIM_CKD_DIV1);
 
   TIM_OCInitStructure.TIM_OCMode = TIM_OCMode_Timing;
   TIM_OCInitStructure.TIM_OutputState = TIM_OutputState_Disable;
   TIM_OCInitStructure.TIM_OCPolarity = TIM_OCPolarity_High;
-  manager_motion_period=period_us;
+  manager_motion_period=(period_us*1000000)>>16;
   TIM_OCInitStructure.TIM_Pulse = manager_motion_period;
   TIM_OC1Init(MOTION_TIMER, &TIM_OCInitStructure);
   TIM_OC1PreloadConfig(MOTION_TIMER, TIM_OCPreload_Disable);
 
   ram_clear_bit(BIOLOID_MM_STATUS,0);
+
+  /* initialize motion modules */
+  action_init(period_us);
+
+  /* initialize balance parameters */
+  for(i=0;i<MANAGER_MAX_NUM_SERVOS;i++)
+    balance_offset[i]=0;
+  balance_enabled=0x01;
 }
 
 uint16_t manager_get_period(void)
@@ -290,11 +297,17 @@ uint16_t manager_get_period(void)
 
 void manager_set_period(uint16_t period_us)
 {
-  manager_motion_period=period_us;
+  manager_motion_period=(period_us*1000000)>>16;
+  ram_write_word(BIOLOID_MM_PERIOD_H,period_us);
+  action_set_period(period_us);
 }
 
 inline void manager_enable(void)
 {
+  uint16_t capture;
+
+  capture = TIM_GetCapture1(MOTION_TIMER);
+  TIM_SetCompare1(MOTION_TIMER, capture + manager_motion_period);
   TIM_Cmd(MOTION_TIMER, ENABLE);
   TIM_ITConfig(MOTION_TIMER, TIM_IT_CC1, ENABLE);
   ram_set_bit(BIOLOID_MM_STATUS,0);
@@ -312,6 +325,20 @@ inline uint8_t manager_is_enabled(void)
   return ram_data[BIOLOID_MM_STATUS]&0x01;
 }
 
+void manager_enable_balance(void)
+{
+  ram_set_bit(BIOLOID_MM_STATUS,1);
+  ram_set_bit(BIOLOID_MM_CONTROL,1);
+  balance_enabled=0x01;
+}
+
+void manager_disable_balance(void)
+{
+  ram_clear_bit(BIOLOID_MM_STATUS,1);
+  ram_clear_bit(BIOLOID_MM_CONTROL,1);
+  balance_enabled=0x00;
+}
+
 inline uint8_t manager_get_num_servos(void)
 {
   return manager_num_servos;
@@ -319,8 +346,18 @@ inline uint8_t manager_get_num_servos(void)
 
 void manager_set_module(uint8_t servo_id,TModules module)
 {
+  uint8_t byte;
+
   if(servo_id>=0 && servo_id<MANAGER_MAX_NUM_SERVOS)
+  {
     manager_servos[servo_id].module=module;
+    ram_read_byte(BIOLOID_MM_MODULE_EN0+servo_id/2,&byte);
+    if(servo_id%2)
+      byte=(byte&0xF8)+((uint8_t)module);
+    else
+      byte=(byte&0x8F)+(((uint8_t)module)<<4);
+    ram_write_byte(BIOLOID_MM_MODULE_EN0+servo_id/2,byte);
+  }
 }
 
 inline TModules manager_get_module(uint8_t servo_id)
@@ -333,33 +370,38 @@ inline TModules manager_get_module(uint8_t servo_id)
 
 inline void manager_enable_servo(uint8_t servo_id)
 {
+  uint8_t byte;
+
   if(servo_id>=0 && servo_id<MANAGER_MAX_NUM_SERVOS)
+  {
     manager_servos[servo_id].enabled=0x01;
+    ram_read_byte(BIOLOID_MM_MODULE_EN0+servo_id/2,&byte);
+    if(servo_id%2)
+      byte|=0x08;
+    else
+      byte|=0x80;
+    ram_write_byte(BIOLOID_MM_MODULE_EN0+servo_id/2,byte);
+  }
 }
 
 inline void manager_disable_servo(uint8_t servo_id)
 {
+  uint8_t byte;
+
   if(servo_id>=0 && servo_id<MANAGER_MAX_NUM_SERVOS)
+  {
     manager_servos[servo_id].enabled=0x00;
+    ram_read_byte(BIOLOID_MM_MODULE_EN0+servo_id/2,&byte);
+    if(servo_id%2)
+      byte&=0xF7;
+    else
+      byte&=0x7F;
+    ram_write_byte(BIOLOID_MM_MODULE_EN0+servo_id/2,byte);
+  }
 }
 
 void manager_disable_servos(void)
 {
-  uint8_t servo_ids[MANAGER_MAX_NUM_SERVOS];
-  uint8_t data[256];
-  uint8_t i,num=0;
-
-  for(i=0;i<MANAGER_MAX_NUM_SERVOS;i++)
-  {
-    if(!manager_servos[i].enabled)
-    {
-      servo_ids[num]=manager_servos[i].id;
-      data[num]=0x00;
-      num++;
-    }
-  }
-  if(num>0)
-    dyn_master_sync_write(num,servo_ids,P_TORQUE_ENABLE,1,data);
 }
 
 inline uint8_t manager_is_servo_enabled(uint8_t servo_id)
@@ -370,3 +412,13 @@ inline uint8_t manager_is_servo_enabled(uint8_t servo_id)
     return 0x00;
 }
 
+inline int16_t manager_get_cw_angle_limit(uint8_t servo_id)
+{
+  return manager_servos[servo_id].cw_angle_limit;
+}
+
+inline int16_t manager_get_ccw_angle_limit(uint8_t servo_id)
+{
+  return manager_servos[servo_id].ccw_angle_limit;
+}
+