diff --git a/include/action.h b/include/action.h
index 29e4eff35f432037a2857d8a118833f3e1420acb..afaa86108ce32495aee2d520c819f092ff71221e 100755
--- a/include/action.h
+++ b/include/action.h
@@ -2,9 +2,6 @@
 #define _ACTION_H
 
 #include "stm32f10x.h"
-#include "motion_manager.h"
-
-extern int64_t action_current_angles[MANAGER_MAX_NUM_SERVOS];
 
 // public functions
 void action_init(uint16_t period);
diff --git a/include/comm.h b/include/comm.h
new file mode 100644
index 0000000000000000000000000000000000000000..c5e404e9005fd2c665384daab6bcfb129c862a87
--- /dev/null
+++ b/include/comm.h
@@ -0,0 +1,10 @@
+#ifndef _COMM_H
+#define _COMM_H
+
+#include "stm32f10x.h"
+
+// public functions
+void comm_init(void);
+void comm_start(void);
+
+#endif
diff --git a/include/dyn_servos.h b/include/dyn_servos.h
index 775f26d6ccbede393c8efa80a9da52f2c6c558e9..eb6619d703503564800a70cc96362e49913d21bd 100755
--- a/include/dyn_servos.h
+++ b/include/dyn_servos.h
@@ -12,6 +12,7 @@
 #define       SERVO_MX64      0x0136
 #define       SERVO_EX106     0x006B
 #define       SERVO_MX106     0x0140
+#define       SERVO_XL320     0x015E
 
 // Servo register map
 typedef enum{
@@ -88,4 +89,50 @@ typedef enum{
   P_D_ERROR_OUT_L             = 66,
   P_D_ERROR_OUT_H             = 67}TDynServo;
 
+typedef enum{
+  XL_MODEL_NUMBER_L            = 0,
+  XL_MODEL_NUMBER_H            = 1,
+  XL_VERSION                   = 2,
+  XL_ID                        = 3,
+  XL_BAUD_RATE                 = 4,
+  XL_RETURN_DELAY_TIME         = 5,
+  XL_CW_ANGLE_LIMIT_L          = 6,
+  XL_CW_ANGLE_LIMIT_H          = 7,
+  XL_CCW_ANGLE_LIMIT_L         = 8,
+  XL_CCW_ANGLE_LIMIT_H         = 9,
+  XL_SYSTEM_DATA2              = 10,
+  XL_CONTROL_MODE              = 11,
+  XL_HIGH_LIMIT_TEMPERATURE    = 12,
+  XL_LOW_LIMIT_VOLTAGE         = 13,
+  XL_HIGH_LIMIT_VOLTAGE        = 14,
+  XL_MAX_TORQUE_L              = 15,
+  XL_MAX_TORQUE_H              = 16,
+  XL_RETURN_LEVEL              = 17,
+  XL_ALARM_SHUTDOWN            = 18,
+  XL_TORQUE_ENABLE             = 24,
+  XL_LED                       = 25,
+  XL_D_GAIN                    = 27,
+  XL_I_GAIN                    = 28,
+  XL_P_GAIN                    = 29,
+  XL_GOAL_POSITION_L           = 30,
+  XL_GOAL_POSITION_H           = 31,
+  XL_MOVING_SPEED_L            = 32,
+  XL_MOVING_SPEED_H            = 33,
+  XL_TORQUE_LIMIT_L            = 35,
+  XL_TORQUE_LIMIT_H            = 36,
+  XL_PRESENT_POSITION_L        = 37,
+  XL_PRESENT_POSITION_H        = 38,
+  XL_PRESENT_SPEED_L           = 39,
+  XL_PRESENT_SPEED_H           = 40,
+  XL_PRESENT_LOAD_L            = 41,
+  XL_PRESENT_LOAD_H            = 42,
+  XL_PRESENT_VOLTAGE           = 45,
+  XL_PRESENT_TEMPERATURE       = 46,
+  XL_REGISTERED_INSTRUCTION    = 47,
+  XL_MOVING                    = 49,
+  XL_HARDWARE_ERROR_STATUS     = 50,
+  XL_PUNCH_L                   = 51,
+  XL_PUNCH_H                   = 51}TXLServo;
+
+
 #endif
diff --git a/include/dynamixel_master_uart_dma.h b/include/dynamixel_master_uart_dma.h
index 1a2916a3057bbbce52ced79291e9c644ee969a7e..cf0e788b23dd0109244c420b1060130aa72876be 100755
--- a/include/dynamixel_master_uart_dma.h
+++ b/include/dynamixel_master_uart_dma.h
@@ -3,25 +3,43 @@
 
 #include "stm32f10x.h"
 #include "dynamixel.h"
+#include "dynamixel2.h"
+
+typedef struct{
+  uint8_t id;
+  uint8_t address;
+  uint8_t length;
+  uint8_t *data;
+}TBulkData;
 
 // dynamixel master functions
 void dyn_master_init(void);
-void dyn_master_flush(void);
+void dyn_master_reset(void);
 void dyn_master_set_timeout(uint16_t timeout_ms);
 void dyn_master_scan(uint8_t *num,uint8_t *ids);
+void dyn2_master_scan(uint8_t *num,uint8_t *ids);
 inline void dyn_master_enable_power(void);
 inline void dyn_master_disable_power(void);
 uint8_t dyn_master_is_power_enabled(void);
 uint8_t dyn_master_ping(uint8_t id);
+uint8_t dyn2_master_ping(uint8_t id);
 uint8_t dyn_master_read_byte(uint8_t id,uint8_t address,uint8_t *data);
+uint8_t dyn2_master_read_byte(uint8_t id,uint16_t address,uint8_t *data);
 uint8_t dyn_master_read_word(uint8_t id,uint8_t address,uint16_t *data);
+uint8_t dyn2_master_read_word(uint8_t id,uint16_t address,uint16_t *data);
 uint8_t dyn_master_read_table(uint8_t id,uint8_t address,uint8_t length,uint8_t *data);
+uint8_t dyn2_master_read_table(uint8_t id,uint16_t address,uint16_t length,uint8_t *data);
 uint8_t dyn_master_write_byte(uint8_t id, uint8_t address, uint8_t data);
+uint8_t dyn2_master_write_byte(uint8_t id, uint16_t address, uint8_t data);
 uint8_t dyn_master_write_word(uint8_t id, uint8_t address, uint16_t data);
+uint8_t dyn2_master_write_word(uint8_t id, uint16_t address, uint16_t data);
 uint8_t dyn_master_write_table(uint8_t id, uint8_t address, uint8_t length, uint8_t *data);
+uint8_t dyn2_master_write_table(uint8_t id, uint16_t address, uint16_t length, uint8_t *data);
 uint8_t dyn_master_reg_write(uint8_t id, uint8_t address, uint8_t length, uint8_t *data);
 void dyn_master_action(void);
 void dyn_master_sync_write(uint8_t num,uint8_t *ids,uint8_t address, uint8_t length, uint8_t *data);
+void dyn2_master_sync_write(uint8_t num,uint8_t *ids,uint16_t address, uint16_t length, uint8_t *data);
+uint8_t dyn_master_bulk_read(uint8_t num,TBulkData *info);
 // repeater functions
 uint8_t dyn_master_resend_inst_packet(uint8_t *inst_packet, uint8_t *status_packet);
 
diff --git a/include/grippers.h b/include/grippers.h
new file mode 100644
index 0000000000000000000000000000000000000000..af3f37f90de33173e70d983b715acc2e2f51aaf8
--- /dev/null
+++ b/include/grippers.h
@@ -0,0 +1,14 @@
+#ifndef _GRIPPERS_H
+#define _GRIPPERS_H
+
+#include "stm32f10x.h"
+
+typedef enum {LEFT_GRIPPER=0,RIGHT_GRIPPER=1} grippers_t;
+
+/* public functions */
+void grippers_init(void);
+void grippers_set_force(grippers_t gripper,uint8_t force);
+void grippers_open(grippers_t gripper);
+void grippers_close(grippers_t gripper);
+
+#endif
diff --git a/include/head_tracking.h b/include/head_tracking.h
new file mode 100644
index 0000000000000000000000000000000000000000..ddba99bcf5aed0eb8862c827514aa58039072480
--- /dev/null
+++ b/include/head_tracking.h
@@ -0,0 +1,23 @@
+#ifndef _HEAD_TRACKING_H
+#define _HEAD_TRACKINH_H
+
+#include "stm32f10x.h"
+
+// public functions
+void head_tracking_init(uint16_t period);
+void head_tracking_start(void);
+void head_tracking_stop(void);
+void head_tracking_set_period(int16_t period);
+void head_tracking_set_pan_p(uint8_t value);
+void head_tracking_set_pan_i(uint8_t value);
+void head_tracking_set_pan_d(uint8_t value);
+void head_tracking_set_tilt_p(uint8_t value);
+void head_tracking_set_tilt_i(uint8_t value);
+void head_tracking_set_tilt_d(uint8_t value);
+void head_tracking_set_target_pan(int16_t target);
+void head_tracking_set_target_tilt(int16_t target);
+
+// motion manager process function
+void head_tracking_process(void);
+
+#endif
diff --git a/include/imu.h b/include/imu.h
index ef91bc9a17d61977a7815d79e9e19a64893f876b..65909fa11cd71face5c6b9ec144ae790fcf3e433 100755
--- a/include/imu.h
+++ b/include/imu.h
@@ -7,6 +7,8 @@
 void imu_init(void);
 void imu_start(void);
 void imu_stop(void);
-void imu_accel_get_data(void);
+
+void imu_start_gyro_cal(void);
+void imu_get_gyro_data(int32_t *x, int32_t *y, int32_t *z);
 
 #endif
diff --git a/include/joint_motion.h b/include/joint_motion.h
new file mode 100644
index 0000000000000000000000000000000000000000..654f9d9df99b2592984781b34de134550fa21828
--- /dev/null
+++ b/include/joint_motion.h
@@ -0,0 +1,20 @@
+#ifndef _JOINT_MOTION_H
+#define _JOINT_MOTION_H
+
+#include "stm32f10x.h"
+
+// public functions
+void joint_motion_init(uint16_t period);
+void joint_motion_set_period(uint16_t period);
+uint16_t joint_motion_get_period(void);
+inline void joint_motion_load_target_angle(uint8_t servo_id, int16_t angle);
+inline void joint_motion_load_target_speed(uint8_t servo_id, uint8_t speed);
+inline void joint_motion_load_target_accel(uint8_t servo_id, uint8_t accel);
+void joint_motion_start(void);
+void joint_motion_stop(void);
+uint8_t are_joints_moving(void);
+
+// motion manager interface functions
+void joint_motion_process(void);
+
+#endif
diff --git a/include/motion_manager.h b/include/motion_manager.h
index 050812e51c95429eaba92c2a653af63cc7f5ecc9..0d6c86fee6bc0b32ac3de4635d26007d11063a61 100755
--- a/include/motion_manager.h
+++ b/include/motion_manager.h
@@ -8,7 +8,9 @@
 typedef enum{MM_NONE          = 0,
              MM_ACTION        = 1,
              MM_WALKING       = 2,
-             MM_JOINTS        = 3} TModules;
+             MM_JOINTS        = 3,
+             MM_HEAD          = 4,
+             MM_GRIPPER       = 5} TModules;
 
 // default joint identifiers
 enum{
@@ -31,7 +33,11 @@ enum{
   R_ANKLE_ROLL         = 17,
   L_ANKLE_ROLL         = 18,
   HEAD_PAN             = 19,
-  HEAD_TILT            = 20
+  HEAD_TILT            = 20,
+  R_WRIST              = 23,
+  L_WRIST              = 22,
+  R_GRIPPER            = 21,
+  L_GRIPPER            = 24
 };
 
 // servo information structure
@@ -47,8 +53,14 @@ typedef struct{
   uint16_t current_value;
   TModules module;
   uint8_t enabled;
+  int16_t cw_angle_limit;
+  int16_t ccw_angle_limit;
+  uint8_t dyn_version;
 }TServoInfo;
 
+// public variables
+extern int64_t current_angles[MANAGER_MAX_NUM_SERVOS];
+
 // public functions
 void manager_init(uint16_t period_us);
 inline uint16_t manager_get_period(void);
@@ -56,6 +68,8 @@ inline void manager_set_period(uint16_t period_us);
 inline void manager_enable(void);
 inline void manager_disable(void);
 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);
 inline TModules manager_get_module(uint8_t servo_id);
@@ -63,5 +77,12 @@ inline void manager_enable_servo(uint8_t servo_id);
 inline void manager_disable_servo(uint8_t servo_id);
 void manager_disable_servos(void);
 inline uint8_t manager_is_servo_enabled(uint8_t servo_id);
+inline int16_t manager_get_cw_angle_limit(uint8_t servo_id);
+inline int16_t manager_get_ccw_angle_limit(uint8_t servo_id);
+void manager_set_knee_gain(uint8_t gain);
+void manager_set_ankle_pitch_gain(uint8_t gain);
+void manager_set_hip_roll_gain(uint8_t gain);
+void manager_set_ankle_roll_gain(uint8_t gain);
+uint8_t manager_get_dyn_version(uint8_t servo_id);
 
 #endif
diff --git a/include/ram.h b/include/ram.h
index 335bdaed2b87735e3936547351030b8cf3e1eef8..737a71b94a99266b57b9156f03cc16cfebf2f419 100755
--- a/include/ram.h
+++ b/include/ram.h
@@ -17,7 +17,8 @@ typedef enum {
   DARWIN_ID                      = DEVICE_ID_OFFSET,
   DARWIN_BAUD_RATE               = BAUDRATE_OFFSET,
   DARWIN_RETURN_DELAY_TIME       = RETURN_DELAY_OFFSET,
-  DARWIN_MM_PERIOD               = MM_PERIOD_OFFSET,
+  DARWIN_MM_PERIOD_L             = MM_PERIOD_OFFSET,
+  DARWIN_MM_PERIOD_H             = MM_PERIOD_OFFSET+1,
   DARWIN_RETURN_LEVEL            = RETURN_LEVEL_OFFSET,
   DARWIN_DXL_POWER               = 0x18,
   DARWIN_LED_PANNEL              = 0x19,
@@ -146,47 +147,114 @@ typedef enum {
   DARWIN_MM_SERVO25_CUR_POS_H    = 0x94,
   DARWIN_MM_SERVO26_CUR_POS_L    = 0x95,
   DARWIN_MM_SERVO26_CUR_POS_H    = 0x96,
-  DARWIN_MM_SERVO27_CUR_POS_L    = 0x97,
-  DARWIN_MM_SERVO27_CUR_POS_H    = 0x98,
-  DARWIN_MM_SERVO28_CUR_POS_L    = 0x99,
-  DARWIN_MM_SERVO28_CUR_POS_H    = 0x9A,
-  DARWIN_MM_SERVO29_CUR_POS_L    = 0x9B,
-  DARWIN_MM_SERVO29_CUR_POS_H    = 0x9C,
-  DARWIN_MM_SERVO30_CUR_POS_L    = 0x9D,
-  DARWIN_MM_SERVO30_CUR_POS_H    = 0x9E,
-  DARWIN_ACTION_PAGE             = 0x9F,
-  DARWIN_ACTION_CONTROL          = 0xA0,
-  DARWIN_ACTION_STATUS           = 0xA1,
-  DARWIN_X_OFFSET                = 0xA2,
-  DARWIN_Y_OFFSET                = 0xA3,
-  DARWIN_Z_OFFSET                = 0xA4,
-  DARWIN_ROLL                    = 0xA5,
-  DARWIN_PITCH                   = 0xA6,
-  DARWIN_YAW                     = 0xA7,
-  DARWIN_HIP_PITCH_OFF_L         = 0xA8,
-  DARWIN_HIP_PITCH_OFF_H         = 0xA9,
-  DARWIN_PERIOD_TIME_L           = 0xAA,
-  DARWIN_PERIOD_TIME_H           = 0xAB,
-  DARWIN_DSP_RATIO               = 0xAC,
-  DARWIN_STEP_FW_BW_RATIO        = 0xAD,
-  DARWIN_STEP_FW_BW              = 0xAE,
-  DARWIN_STEP_LEFT_RIGHT         = 0xAF,
-  DARWIN_STEP_DIRECTION          = 0xB1,
-  DARWIN_FOOT_HEIGHT             = 0xB2,
-  DARWIN_SWING_RIGHT_LEFT        = 0xB3,
-  DARWIN_SWING_TOP_DOWN          = 0xB4,
-  DARWIN_PELVIS_OFFSET           = 0xB5,
-  DARWIN_ARM_SWING_GAIN          = 0xB6,
-  DARWIN_BAL_KNEE_GAIN           = 0xB7,
-  DARWIN_BAL_ANKLE_PITCH_GAIN    = 0xB8,
-  DARWIN_BAL_HIP_ROLL_GAIN       = 0xB9,
-  DARWIN_BAL_ANKLE_ROLL_GAIN     = 0xBA,
-  DARWIN_P_GAIN                  = 0xBB,
-  DARWIN_D_GAIN                  = 0xBC,
-  DARWIN_I_GAIN                  = 0xBD
+  DARWIN_ACTION_PAGE             = 0x97,
+  DARWIN_ACTION_CONTROL          = 0x98,
+  DARWIN_ACTION_STATUS           = 0x99,
+  DARWIN_X_OFFSET                = 0x9A,
+  DARWIN_Y_OFFSET                = 0x9B,
+  DARWIN_Z_OFFSET                = 0x9C,
+  DARWIN_ROLL                    = 0x9D,
+  DARWIN_PITCH                   = 0x9E,
+  DARWIN_YAW                     = 0x9F,
+  DARWIN_HIP_PITCH_OFF_L         = 0xA0,
+  DARWIN_HIP_PITCH_OFF_H         = 0xA1,
+  DARWIN_PERIOD_TIME_L           = 0xA2,
+  DARWIN_PERIOD_TIME_H           = 0xA3,
+  DARWIN_DSP_RATIO               = 0xA4,
+  DARWIN_STEP_FW_BW_RATIO        = 0xA5,
+  DARWIN_STEP_FW_BW              = 0xA6,
+  DARWIN_STEP_LEFT_RIGHT         = 0xA7,
+  DARWIN_STEP_DIRECTION          = 0xA8,
+  DARWIN_FOOT_HEIGHT             = 0xA9,
+  DARWIN_SWING_RIGHT_LEFT        = 0xAA,
+  DARWIN_SWING_TOP_DOWN          = 0xAB,
+  DARWIN_PELVIS_OFFSET           = 0xAC,
+  DARWIN_ARM_SWING_GAIN          = 0xAD,
+  DARWIN_P_GAIN                  = 0xAE,
+  DARWIN_D_GAIN                  = 0xAF,
+  DARWIN_I_GAIN                  = 0xB0,
+  DARWIN_MAX_ACC                 = 0xB1,
+  DARWIN_MAX_ROT_ACC             = 0xB2,
+  DARWIN_MM_KNEE_GAIN            = 0xB3,
+  DARWIN_MM_ANKLE_PITCH_GAIN     = 0xB4,
+  DARWIN_MM_HIP_ROLL_GAIN        = 0xB5,
+  DARWIN_MM_ANKLE_ROLL_GAIN      = 0xB6,
+  DARWIN_SERVO_0_SPEED           = 0xB7,
+  DARWIN_SERVO_1_SPEED           = 0xB8,
+  DARWIN_SERVO_2_SPEED           = 0xB9,
+  DARWIN_SERVO_3_SPEED           = 0xBA,
+  DARWIN_SERVO_4_SPEED           = 0xBB,
+  DARWIN_SERVO_5_SPEED           = 0xBC,
+  DARWIN_SERVO_6_SPEED           = 0xBD,
+  DARWIN_SERVO_7_SPEED           = 0xBE,
+  DARWIN_SERVO_8_SPEED           = 0xBF, 
+  DARWIN_SERVO_9_SPEED           = 0xC0,
+  DARWIN_SERVO_10_SPEED          = 0xC1,
+  DARWIN_SERVO_11_SPEED          = 0xC2,
+  DARWIN_SERVO_12_SPEED          = 0xC3,
+  DARWIN_SERVO_13_SPEED          = 0xC4,
+  DARWIN_SERVO_14_SPEED          = 0xC5,
+  DARWIN_SERVO_15_SPEED          = 0xC6,
+  DARWIN_SERVO_16_SPEED          = 0xC7,
+  DARWIN_SERVO_17_SPEED          = 0xC8,
+  DARWIN_SERVO_18_SPEED          = 0xC9,
+  DARWIN_SERVO_19_SPEED          = 0xCA,
+  DARWIN_SERVO_20_SPEED          = 0xCB,
+  DARWIN_SERVO_21_SPEED          = 0xCC,
+  DARWIN_SERVO_22_SPEED          = 0xCD,
+  DARWIN_SERVO_23_SPEED          = 0xCE,
+  DARWIN_SERVO_24_SPEED          = 0xCF,
+  DARWIN_SERVO_25_SPEED          = 0xD0,
+  DARWIN_SERVO_26_SPEED          = 0xD1,
+  DARWIN_SERVO_0_ACCEL           = 0xD2,
+  DARWIN_SERVO_1_ACCEL           = 0xD3,
+  DARWIN_SERVO_2_ACCEL           = 0xD4,
+  DARWIN_SERVO_3_ACCEL           = 0xD5,
+  DARWIN_SERVO_4_ACCEL           = 0xD6,
+  DARWIN_SERVO_5_ACCEL           = 0xD7,
+  DARWIN_SERVO_6_ACCEL           = 0xD8,
+  DARWIN_SERVO_7_ACCEL           = 0xD9,
+  DARWIN_SERVO_8_ACCEL           = 0xDA,
+  DARWIN_SERVO_9_ACCEL           = 0xDB,
+  DARWIN_SERVO_10_ACCEL          = 0xDC,
+  DARWIN_SERVO_11_ACCEL          = 0xDD,
+  DARWIN_SERVO_12_ACCEL          = 0xDE,
+  DARWIN_SERVO_13_ACCEL          = 0xDF,
+  DARWIN_SERVO_14_ACCEL          = 0xE0,
+  DARWIN_SERVO_15_ACCEL          = 0xE1, 
+  DARWIN_SERVO_16_ACCEL          = 0xE2,
+  DARWIN_SERVO_17_ACCEL          = 0xE3, 
+  DARWIN_SERVO_18_ACCEL          = 0xE4,
+  DARWIN_SERVO_19_ACCEL          = 0xE5, 
+  DARWIN_SERVO_20_ACCEL          = 0xE6,
+  DARWIN_SERVO_21_ACCEL          = 0xE7,
+  DARWIN_SERVO_22_ACCEL          = 0xE8,
+  DARWIN_SERVO_23_ACCEL          = 0xE9,
+  DARWIN_SERVO_24_ACCEL          = 0xEA,
+  DARWIN_SERVO_25_ACCEL          = 0xEB,
+  DARWIN_SERVO_26_ACCEL          = 0xEC,
+  DARWIN_JOINTS_CONTROL          = 0xED,
+  DARWIN_JOINTS_STATUS           = 0xEE,
+  DARWIN_HEAD_PAN_L              = 0xEF,
+  DARWIN_HEAD_PAN_H              = 0xF0,
+  DARWIN_HEAD_TILT_L             = 0xF1,
+  DARWIN_HEAD_TILT_H             = 0xF2,
+  DARWIN_HEAD_PAN_P              = 0xF3,
+  DARWIN_HEAD_PAN_I              = 0xF4,
+  DARWIN_HEAD_PAN_D              = 0xF5,
+  DARWIN_HEAD_TILT_P             = 0xF6,
+  DARWIN_HEAD_TILT_I             = 0xF7,
+  DARWIN_HEAD_TILT_D             = 0xF8,
+  DARWIN_HEAD_CONTROL            = 0xF9,
+  DARWIN_HEAD_STATUS             = 0xFA,
+  DARWIN_MM_CUR_PERIOD           = 0xFB,
+  DARWIN_GRIPPER_CONTROL         = 0xFC,
+  DARWIN_GRIPPER_STATUS          = 0xFD,
+  DARWIN_GRIPPER_FORCE_LEFT      = 0xFE,
+  DARWIN_GRIPPER_FORCE_RIGHT     = 0xFF
 } darwin_registers;
 
-#define      RAM_SIZE          256
+#define            RAM_SIZE      ((uint16_t)0x100)
 
 extern uint8_t ram_data[RAM_SIZE];
 
diff --git a/include/walking.h b/include/walking.h
index 0b7e4773c19c089450704b25897cc6a1fe9bd8d7..da933a5efbd30306b01a35ca7e868b4c01d75b6d 100755
--- a/include/walking.h
+++ b/include/walking.h
@@ -2,19 +2,12 @@
 #define _WALKING_H
 
 #include "stm32f10x.h"
-#include "motion_manager.h"
-
-extern int64_t walking_current_angles[MANAGER_MAX_NUM_SERVOS];
 
 // public functions
-void walking_init(void);
-void walking_set_param(uint8_t param_id,uint16_t param_value);
-void walking_enable_autobalance(void);
-void walking_disable_autobalance(void);
-uint8_t walking_is_autobalance_enabled(void);
-void walking_enable_aim(void);
-void walking_disable_aim(void);
-uint8_t walking_is_aim_enabled(void);
+void walking_init(uint16_t period);
+inline void walking_set_period(uint16_t period);
+inline uint16_t walking_get_period(void);
+void walking_set_param(uint8_t param_id,int8_t param_value);
 void walking_start(void);
 void walking_stop(void);
 uint8_t is_walking(void);
diff --git a/src/action.c b/src/action.c
index cc0429d99bb803ffc144cbd5a5e4ae280e5ed584..67ff5ba94832bf05191203d3cb5495eec66b18f2 100755
--- a/src/action.c
+++ b/src/action.c
@@ -1,5 +1,6 @@
 #include "action.h"
 #include "motion_pages.h"
+#include "motion_manager.h"
 #include "ram.h"
 
 #define SPEED_BASE_SCHEDULE 0x00
@@ -13,7 +14,6 @@ TPage action_current_page;
 uint8_t action_current_step_index;
 // angle variables
 int64_t action_moving_angles[PAGE_MAX_NUM_SERVOS];// fixed point 48|16 format
-int64_t action_current_angles[PAGE_MAX_NUM_SERVOS];
 int64_t action_start_angles[PAGE_MAX_NUM_SERVOS];// fixed point 48|16 format
 // speed variables
 int64_t action_start_speed[PAGE_MAX_NUM_SERVOS];// fixed point 48|16 format
@@ -126,46 +126,49 @@ void action_load_next_step(void)
   max_angle=0;
   for(i=0;i<PAGE_MAX_NUM_SERVOS;i++)
   {
-    angle=action_current_page.steps[action_current_step_index].position[i];
-    if(angle==0x5A00)// bigger than 180
-      target_angles=action_current_angles[i];
-    else
-      target_angles=angle<<9;
-    action_moving_angles[i]=target_angles-action_current_angles[i];
-    if(action_end)
-      next_angles=target_angles;
-    else
+    if(manager_get_module(i)==MM_ACTION)
     {
-      if(action_current_step_index==action_current_page.header.stepnum-1)
-        next_angle=action_next_page.steps[0].position[i];
+      angle=action_current_page.steps[action_current_step_index].position[i];
+      if(angle==0x5A00)// bigger than 180
+        target_angles=current_angles[i];
       else
-        next_angle=action_current_page.steps[action_current_step_index+1].position[i];
-      if(next_angle==0x5A00)
+        target_angles=angle<<9;
+      action_moving_angles[i]=target_angles-current_angles[i];
+      if(action_end)
         next_angles=target_angles;
       else
-        next_angles=next_angle<<9;
-    }
-    // check changes in direction
-    if(((action_current_angles[i] < target_angles) && (target_angles < next_angles)) ||
-       ((action_current_angles[i] > target_angles) && (target_angles > next_angles)))
-      dir_change=0x00;
-    else
-      dir_change=0x01;
-    // check the type of ending
-    if(dir_change || action_pause_time>0 || action_end)
-      action_zero_speed_finish[i]=0x01;
-    else
-      action_zero_speed_finish[i]=0x00;
-    // find the maximum angle of motion in speed base schedule
-    if(action_current_page.header.schedule==SPEED_BASE_SCHEDULE)// speed base schedule
-    {
-      // fer el valor absolut
-      if(action_moving_angles[i]<0)
-        tmp_angle=-action_moving_angles[i];
+      {
+        if(action_current_step_index==action_current_page.header.stepnum-1)
+          next_angle=action_next_page.steps[0].position[i];
+        else
+          next_angle=action_current_page.steps[action_current_step_index+1].position[i];
+        if(next_angle==0x5A00)
+          next_angles=target_angles;
+        else
+          next_angles=next_angle<<9;
+      }
+      // check changes in direction
+      if(((current_angles[i] < target_angles) && (target_angles < next_angles)) ||
+         ((current_angles[i] > target_angles) && (target_angles > next_angles)))
+        dir_change=0x00;
+      else
+        dir_change=0x01;
+      // check the type of ending
+      if(dir_change || action_pause_time>0 || action_end)
+        action_zero_speed_finish[i]=0x01;
       else
-        tmp_angle=action_moving_angles[i];
-      if(tmp_angle>max_angle)
-        max_angle=tmp_angle;
+        action_zero_speed_finish[i]=0x00;
+      // find the maximum angle of motion in speed base schedule
+      if(action_current_page.header.schedule==SPEED_BASE_SCHEDULE)// speed base schedule
+      {
+        // fer el valor absolut
+        if(action_moving_angles[i]<0)
+          tmp_angle=-action_moving_angles[i];
+        else
+          tmp_angle=action_moving_angles[i];
+        if(tmp_angle>max_angle)
+          max_angle=tmp_angle;
+      }
     }
   }
   if(action_current_page.header.schedule==SPEED_BASE_SCHEDULE)
@@ -194,16 +197,19 @@ void action_load_next_step(void)
     zero_speed_divider=action_period;
   for(i=0;i<PAGE_MAX_NUM_SERVOS;i++)
   {
-    action_pre_time_initial_speed_angle=(action_start_speed[i]*action_pre_time)>>1;
-    moving_angle=action_moving_angles[i]<<16;
-    if(action_zero_speed_finish[i])
-      action_main_speed[i]=(moving_angle-action_pre_time_initial_speed_angle)/zero_speed_divider;
-    else
-      action_main_speed[i]=(moving_angle-action_pre_time_initial_speed_angle)/non_zero_speed_divider;
-//    if((action_main_speed[i]>>16)>info.max_speed)
-//      action_main_speed[i]=(info.max_speed*65536);
-//    else if((action_main_speed[i]>>16)<-info.max_speed)
-//      action_main_speed[i]=-(info.max_speed*65536);
+    if(manager_get_module(i)==MM_ACTION)
+    {
+      action_pre_time_initial_speed_angle=(action_start_speed[i]*action_pre_time)>>1;
+      moving_angle=action_moving_angles[i]<<16;
+      if(action_zero_speed_finish[i])
+        action_main_speed[i]=(moving_angle-action_pre_time_initial_speed_angle)/zero_speed_divider;
+      else
+        action_main_speed[i]=(moving_angle-action_pre_time_initial_speed_angle)/non_zero_speed_divider;
+//      if((action_main_speed[i]>>16)>info.max_speed)
+//        action_main_speed[i]=(info.max_speed*65536);
+//      else if((action_main_speed[i]>>16)<-info.max_speed)
+//        action_main_speed[i]=-(info.max_speed*65536);
+    }
   }
 }
 
@@ -217,7 +223,7 @@ void action_init(uint16_t period)
   {
     // angle variables
     action_moving_angles[i]=0;// fixed point 48|16 format
-    action_start_angles[i]=action_current_angles[i];
+    action_start_angles[i]=current_angles[i];
     // speed variables
     action_start_speed[i]=0;// fixed point 48|16 format
     action_main_speed[i]=0;// fixed point 48|16 format
@@ -265,6 +271,7 @@ uint8_t action_load_page(uint8_t page_id)
     return 0x00;
   if(!pages_check_checksum(&action_next_page))
     return 0x00;
+  ram_write_byte(DARWIN_ACTION_PAGE,page_id);
 
   return 0x01;
 }
@@ -274,18 +281,20 @@ void action_start_page(void)
   uint8_t i;
 
   for(i=0;i<PAGE_MAX_NUM_SERVOS;i++)
-    action_start_angles[i]=action_current_angles[i];
+    action_start_angles[i]=current_angles[i];
   action_stop=0x00;
   action_current_time=0;
   action_section_time=0; 
   action_current_step_index=-1;
   ram_set_bit(DARWIN_ACTION_STATUS,0);
   action_running=0x01;
+  ram_set_bit(DARWIN_ACTION_CONTROL,0); 
 }
 
 void action_stop_page(void)
 {
   action_stop=0x01;
+  ram_set_bit(DARWIN_ACTION_CONTROL,1); 
 }
 
 uint8_t action_is_running(void)
@@ -315,28 +324,31 @@ void action_process(void)
                        {
                          for(i=0;i<PAGE_MAX_NUM_SERVOS;i++)
                          {
-                           /* the last segment of the pre section */
-                           delta_speed=(action_main_speed[i]-action_start_speed[i]);
-                           current_speed[i]=action_start_speed[i]+delta_speed;
-                           accel_angles[i]=(action_start_speed[i]*action_section_time+((delta_speed*action_section_time)>>1))>>16;
-                           action_current_angles[i]=action_start_angles[i]+accel_angles[i];
-                           /* update of the state */
-                           if(!action_zero_speed_finish[i])
+                           if(manager_get_module(i)==MM_ACTION)
                            {
-                             if((action_step_time-action_pre_time)==0)
-                               main_angles[i]=0;
+                             /* the last segment of the pre section */
+                             delta_speed=(action_main_speed[i]-action_start_speed[i]);
+                             current_speed[i]=action_start_speed[i]+delta_speed;
+                             accel_angles[i]=(action_start_speed[i]*action_section_time+((delta_speed*action_section_time)>>1))>>16;
+                             current_angles[i]=action_start_angles[i]+accel_angles[i];
+                             /* update of the state */
+                             if(!action_zero_speed_finish[i])
+                             {
+                               if((action_step_time-action_pre_time)==0)
+                                 main_angles[i]=0;
+                               else
+                                 main_angles[i]=((action_moving_angles[i]-accel_angles[i])*(action_step_time-(action_pre_time<<1)))/(action_step_time-action_pre_time);
+                               action_start_angles[i]=current_angles[i];
+                             }
                              else
-                               main_angles[i]=((action_moving_angles[i]-accel_angles[i])*(action_step_time-(action_pre_time<<1)))/(action_step_time-action_pre_time);
-                             action_start_angles[i]=action_current_angles[i];
-                           }
-                           else
-                           {
-                             main_angles[i]=action_moving_angles[i]-accel_angles[i]-(((action_main_speed[i]*action_pre_time)>>1)>>16);
-                             action_start_angles[i]=action_current_angles[i];
+                             {
+                               main_angles[i]=action_moving_angles[i]-accel_angles[i]-(((action_main_speed[i]*action_pre_time)>>1)>>16);
+                               action_start_angles[i]=current_angles[i];
+                             }
+                             /* the first step of the main section */
+                             current_angles[i]=action_start_angles[i]+(main_angles[i]*(action_current_time-action_section_time))/(action_step_time-(action_pre_time<<1));
+                             current_speed[i]=action_main_speed[i];
                            }
-                           /* the first step of the main section */
-                           action_current_angles[i]=action_start_angles[i]+(main_angles[i]*(action_current_time-action_section_time))/(action_step_time-(action_pre_time<<1));
-                           current_speed[i]=action_main_speed[i];
                          }
                          action_current_time=action_current_time-action_section_time;
                          action_section_time=action_step_time-(action_pre_time<<1);
@@ -346,10 +358,13 @@ void action_process(void)
                        {
                          for(i=0;i<PAGE_MAX_NUM_SERVOS;i++)
                          {
-                           delta_speed=((action_main_speed[i]-action_start_speed[i])*action_current_time)/action_section_time;
-                           current_speed[i]=action_start_speed[i]+delta_speed;
-                           accel_angles[i]=((action_start_speed[i]*action_current_time)+((delta_speed*action_current_time)>>1))>>16;
-                           action_current_angles[i]=action_start_angles[i]+accel_angles[i];
+                           if(manager_get_module(i)==MM_ACTION)
+                           {
+                             delta_speed=((action_main_speed[i]-action_start_speed[i])*action_current_time)/action_section_time;
+                             current_speed[i]=action_start_speed[i]+delta_speed;
+                             accel_angles[i]=((action_start_speed[i]*action_current_time)+((delta_speed*action_current_time)>>1))>>16;
+                             current_angles[i]=action_start_angles[i]+accel_angles[i];
+                           }
                          }
                          state=ACTION_PRE;
                        }
@@ -358,23 +373,26 @@ void action_process(void)
                         {
                           for(i=0;i<PAGE_MAX_NUM_SERVOS;i++)
                           {
-                            /* last segment of the main section */
-                            action_current_angles[i]=action_start_angles[i]+main_angles[i];
-                            current_speed[i]=action_main_speed[i];
-                            /* update state */
-                            action_start_angles[i]=action_current_angles[i];
-                            main_angles[i]=action_moving_angles[i]-main_angles[i]-accel_angles[i];
-                            /* first segment of the post section */
-                            if(action_zero_speed_finish[i])
+                            if(manager_get_module(i)==MM_ACTION)
                             {
-                              delta_speed=((0.0-action_main_speed[i])*(action_current_time-action_section_time))/action_pre_time;
-                              current_speed[i]=action_main_speed[i]+delta_speed;
-                              action_current_angles[i]=action_start_angles[i]+(((action_main_speed[i]*(action_current_time-action_section_time))+((delta_speed*(action_current_time-action_section_time))>>1))>>16);
-                            }
-                            else
-                            {
-                              action_current_angles[i]=action_start_angles[i]+((main_angles[i]*(action_current_time-action_section_time))/action_pre_time);
+                              /* last segment of the main section */
+                              current_angles[i]=action_start_angles[i]+main_angles[i];
                               current_speed[i]=action_main_speed[i];
+                              /* update state */
+                              action_start_angles[i]=current_angles[i];
+                              main_angles[i]=action_moving_angles[i]-main_angles[i]-accel_angles[i];
+                              /* first segment of the post section */
+                              if(action_zero_speed_finish[i])
+                              {
+                                delta_speed=((0.0-action_main_speed[i])*(action_current_time-action_section_time))/action_pre_time;
+                                current_speed[i]=action_main_speed[i]+delta_speed;
+                                current_angles[i]=action_start_angles[i]+(((action_main_speed[i]*(action_current_time-action_section_time))+((delta_speed*(action_current_time-action_section_time))>>1))>>16);
+                              }
+                              else
+                              {
+                                current_angles[i]=action_start_angles[i]+((main_angles[i]*(action_current_time-action_section_time))/action_pre_time);
+                                current_speed[i]=action_main_speed[i];
+                              }
                             }
                           }
                           action_current_time=action_current_time-action_section_time;
@@ -385,8 +403,11 @@ void action_process(void)
                         {
                           for(i=0;i<PAGE_MAX_NUM_SERVOS;i++)
                           {
-                            action_current_angles[i]=action_start_angles[i]+(main_angles[i]*action_current_time)/action_section_time;
-                            current_speed[i]=action_main_speed[i];
+                            if(manager_get_module(i)==MM_ACTION)
+                            {
+                              current_angles[i]=action_start_angles[i]+(main_angles[i]*action_current_time)/action_section_time;
+                              current_speed[i]=action_main_speed[i];
+                            }
                           }
                           state=ACTION_MAIN;
                         }
@@ -395,21 +416,24 @@ void action_process(void)
                         {
                           for(i=0;i<PAGE_MAX_NUM_SERVOS;i++)
                           {
-                            /* last segment of the post section */
-                            if(action_zero_speed_finish[i])
-                            {
-                              delta_speed=-action_main_speed[i];
-                              current_speed[i]=action_main_speed[i]+delta_speed;
-                              action_current_angles[i]=action_start_angles[i]+(((action_main_speed[i]*action_section_time)+((delta_speed*action_section_time)>>1))>>16);
-                            }
-                            else
+                            if(manager_get_module(i)==MM_ACTION)
                             {
-                              action_current_angles[i]=action_start_angles[i]+main_angles[i];
-                              current_speed[i]=action_main_speed[i];
+                              /* last segment of the post section */
+                              if(action_zero_speed_finish[i])
+                              {
+                                delta_speed=-action_main_speed[i];
+                                current_speed[i]=action_main_speed[i]+delta_speed;
+                                current_angles[i]=action_start_angles[i]+(((action_main_speed[i]*action_section_time)+((delta_speed*action_section_time)>>1))>>16);
+                              }
+                              else
+                              {
+                                current_angles[i]=action_start_angles[i]+main_angles[i];
+                                current_speed[i]=action_main_speed[i];
+                              }
+                              /* update state */
+                              action_start_angles[i]=current_angles[i];
+                              action_start_speed[i]=current_speed[i];
                             }
-                            /* update state */
-                            action_start_angles[i]=action_current_angles[i];
-                            action_start_speed[i]=current_speed[i];
                           }
                           /* load the next step */
                           if(action_pause_time==0)
@@ -428,10 +452,13 @@ void action_process(void)
                               /* first step of the next section */
                               for(i=0;i<PAGE_MAX_NUM_SERVOS;i++)
                               {
-                                delta_speed=((action_main_speed[i]-action_start_speed[i])*(action_current_time-action_section_time))/action_pre_time;
-                                current_speed[i]=action_start_speed[i]+delta_speed;
-                                accel_angles[i]=(((action_start_speed[i]*(action_current_time-action_section_time))+((delta_speed*(action_current_time-action_section_time))>>1))>>16);
-                                action_current_angles[i]=action_start_angles[i]+accel_angles[i];
+                                if(manager_get_module(i)==MM_ACTION)
+                                {
+                                  delta_speed=((action_main_speed[i]-action_start_speed[i])*(action_current_time-action_section_time))/action_pre_time;
+                                  current_speed[i]=action_start_speed[i]+delta_speed;
+                                  accel_angles[i]=(((action_start_speed[i]*(action_current_time-action_section_time))+((delta_speed*(action_current_time-action_section_time))>>1))>>16);
+                                  current_angles[i]=action_start_angles[i]+accel_angles[i];
+                                }
                               }
                               action_current_time=action_current_time-action_section_time;
                               action_section_time=action_pre_time;
@@ -449,16 +476,19 @@ void action_process(void)
                         {
                           for(i=0;i<PAGE_MAX_NUM_SERVOS;i++)
                           {
-                            if(action_zero_speed_finish[i])
+                            if(manager_get_module(i)==MM_ACTION)
                             {
-                              delta_speed=((0.0-action_main_speed[i])*action_current_time)/action_section_time;
-                              current_speed[i]=action_main_speed[i]+delta_speed;
-                              action_current_angles[i]=action_start_angles[i]+(((action_main_speed[i]*action_current_time)+((delta_speed*action_current_time)>>1))>>16);
-                            }
-                            else
-                            {
-                              action_current_angles[i]=action_start_angles[i]+(main_angles[i]*action_current_time)/action_section_time;
-                              current_speed[i]=action_main_speed[i];
+                              if(action_zero_speed_finish[i])
+                              {
+                                delta_speed=((0.0-action_main_speed[i])*action_current_time)/action_section_time;
+                                current_speed[i]=action_main_speed[i]+delta_speed;
+                                current_angles[i]=action_start_angles[i]+(((action_main_speed[i]*action_current_time)+((delta_speed*action_current_time)>>1))>>16);
+                              }
+                              else
+                              {
+                                current_angles[i]=action_start_angles[i]+(main_angles[i]*action_current_time)/action_section_time;
+                                current_speed[i]=action_main_speed[i];
+                              }
                             }
                           }
                           state=ACTION_POST;
@@ -485,7 +515,7 @@ void action_process(void)
                                delta_speed=((action_main_speed[i]-action_start_speed[i])*(action_current_time-action_section_time))/action_pre_time;
                                current_speed[i]=action_start_speed[i]+delta_speed;
                                accel_angles[i]=(((action_start_speed[i]*(action_current_time-action_section_time))+((delta_speed*(action_current_time-action_section_time))>>1))>>16);
-                               action_current_angles[i]=action_start_angles[i]+accel_angles[i];
+                               current_angles[i]=action_start_angles[i]+accel_angles[i];
                              }
                              action_current_time=action_current_time-action_section_time;
                              action_section_time=action_pre_time;
diff --git a/src/adc_dma.c b/src/adc_dma.c
index 0076e5f32ace44c880835fac0258df7719291350..f4549cfedcbb9c8f68e2cd02d65a3ad950471522 100755
--- a/src/adc_dma.c
+++ b/src/adc_dma.c
@@ -99,16 +99,16 @@ void adc_init(void)
   RCC_AHBPeriphClockCmd(RCC_AHBPeriph_DMA1, ENABLE);
   RCC_APB2PeriphClockCmd(RCC_APB2Periph_ADC1, ENABLE);
   RCC_APB2PeriphClockCmd(RCC_APB2Periph_ADC2, ENABLE);
-  RCC_APB2PeriphClockCmd(ADC1_CH1_PORT_CLK | ADC1_CH2_PORT_CLK | ADC1_CH3_PORT_CLK | ADC1_CH4_PORT_CLK | 
+  RCC_APB2PeriphClockCmd(ADC1_CH2_PORT_CLK | ADC1_CH3_PORT_CLK | ADC1_CH4_PORT_CLK | 
                          ADC1_CH5_PORT_CLK | ADC1_CH6_PORT_CLK | ADC1_CH7_PORT_CLK | ADC1_CH8_PORT_CLK, ENABLE);
   RCC_APB2PeriphClockCmd(ADC2_CH1_PORT_CLK | ADC2_CH2_PORT_CLK | ADC2_CH3_PORT_CLK | ADC2_CH4_PORT_CLK |
-                         ADC2_CH1_PORT_CLK | ADC2_CH2_PORT_CLK | ADC2_CH3_PORT_CLK | ADC2_CH4_PORT_CLK, ENABLE);
+                         ADC2_CH5_PORT_CLK | ADC2_CH6_PORT_CLK | ADC2_CH7_PORT_CLK | ADC2_CH8_PORT_CLK, ENABLE);
 
   /* DMA Config */
   DMA_InitStructure.DMA_MemoryBaseAddr = (uint32_t)&ram_data[DARWIN_MIC1_L];
   DMA_InitStructure.DMA_PeripheralBaseAddr = (uint32_t)ADC_CCR_ADDRESS;
   DMA_InitStructure.DMA_DIR = DMA_DIR_PeripheralSRC;
-  DMA_InitStructure.DMA_BufferSize = 32;
+  DMA_InitStructure.DMA_BufferSize = 30;
   DMA_InitStructure.DMA_PeripheralInc = DMA_PeripheralInc_Disable;
   DMA_InitStructure.DMA_MemoryInc = DMA_MemoryInc_Enable;
   DMA_InitStructure.DMA_PeripheralDataSize = DMA_PeripheralDataSize_Word;
diff --git a/src/cm730_fw.c b/src/cm730_fw.c
index e233c0dbf3c590b6cd628360c05dcebcf98db931..3e3aed3627e23bb28ed5ab02680ffb96bc699a9c 100755
--- a/src/cm730_fw.c
+++ b/src/cm730_fw.c
@@ -2,139 +2,24 @@
 #include "gpio.h"
 #include "time.h"
 #include "eeprom.h"
-#include "motion_pages.h"
 #include "motion_manager.h"
 #include "action.h"
 #include "walking.h"
+#include "joint_motion.h"
+#include "head_tracking.h"
+#include "grippers.h"
 #include "dynamixel_slave_uart_dma.h"
 #include "dynamixel_master_uart_dma.h"
 #include "ram.h"
 #include "imu.h"
 #include "adc_dma.h"
-
-uint8_t read_operation(uint8_t address, uint8_t length, uint8_t *data)
-{
-  uint8_t error;
-
-  // pre-read operation
-  // read operation
-  error=ram_read_table(address,length,data);
-  // post-read operation
-  if(ram_in_range(DARWIN_PUSHBUTTON,DARWIN_PUSHBUTTON,address,length))
-    ram_data[DARWIN_PUSHBUTTON]=0x00;
-
-  return error;
-}
-
-uint8_t write_operation(uint8_t address,uint8_t length, uint8_t *data)
-{
-  uint8_t error,byte_value,i,j,do_op=0x00;
-  uint16_t word_value;
-
-  // pre-write operation
-  if(ram_in_range(DARWIN_DXL_POWER,DARWIN_DXL_POWER,address,length))
-  {
-    if(data[DARWIN_DXL_POWER-address]&0x01) dyn_master_enable_power();
-    else dyn_master_disable_power();
-  }
-  if(ram_in_range(DARWIN_LED_PANNEL,DARWIN_LED_PANNEL,address,length))
-  {
-    byte_value=data[DARWIN_LED_PANNEL-address];
-    if(byte_value&0x01)
-      gpio_set_led(LED_2);
-    else
-      gpio_clear_led(LED_2);
-    if(byte_value&0x02)
-      gpio_set_led(LED_3);
-    else
-      gpio_clear_led(LED_3);
-    if(byte_value&0x04)
-      gpio_set_led(LED_4);
-    else
-      gpio_clear_led(LED_4);
-    if(byte_value&0x08)
-      gpio_set_led(LED_TX);
-    else
-      gpio_clear_led(LED_TX);
-    if(byte_value&0x10)
-      gpio_set_led(LED_RX);
-    else
-      gpio_clear_led(LED_RX);
-  }
-  if(ram_in_range(DARWIN_MM_PERIOD,DARWIN_MM_PERIOD+1,address,length))
-  {
-    word_value=data[DARWIN_MM_PERIOD-address]+(data[DARWIN_MM_PERIOD+1-address]<<8);
-    action_set_period(word_value);
-    word_value=(word_value*1000000)>>16;
-    manager_set_period(word_value);
-  }
-  for(i=DARWIN_MM_MODULE_EN0,j=0;i<=DARWIN_MM_MODULE_EN15;i++,j+=2)
-  {
-    do_op=0x01;
-    if(ram_in_range(i,i,address,length))
-    {
-      byte_value=data[i-address];
-      if(byte_value&0x80) manager_enable_servo(j);
-      else manager_disable_servo(j);
-      manager_set_module(j,(byte_value&0x70)>>4);
-      if(byte_value&0x08) manager_enable_servo(j+1);
-      else manager_disable_servo(j+1);
-      manager_set_module(j+1,byte_value&0x07);
-    }
-  }
-  if(do_op)
-  {
-    do_op=0x00;
-    manager_disable_servos();
-  }
-  if(ram_in_range(DARWIN_MM_CONTROL,DARWIN_MM_CONTROL,address,length))
-  {
-    if(data[DARWIN_MM_CONTROL-address]&0x01)
-      manager_enable();
-    else
-      manager_disable();
-  }
-  if(ram_in_range(DARWIN_ACTION_CONTROL,DARWIN_ACTION_CONTROL,address,length))
-  {
-    if(data[DARWIN_ACTION_CONTROL-address]&0x01)
-      action_start_page();
-    if(data[DARWIN_ACTION_CONTROL-address]&0x02)
-      action_stop_page();
-  }
-  if(ram_in_range(DARWIN_ACTION_PAGE,DARWIN_ACTION_PAGE,address,length))// load the page identifier 
-    action_load_page(data[DARWIN_ACTION_PAGE-address]);
-  if(ram_in_range(DARWIN_WALK_CONTROL,DARWIN_WALK_CONTROL,address,length))
-  {
-    if(data[DARWIN_WALK_CONTROL-address]&0x01)
-      walking_start();
-    if(data[DARWIN_WALK_CONTROL-address]&0x02)
-      walking_stop();
-  }
-  if(ram_in_range(DARWIN_IMU_CONTROL,DARWIN_IMU_CONTROL,address,length))// load the page identifier 
-  {
-    if(data[DARWIN_IMU_CONTROL-address]&0x01)
-      imu_start();
-    else
-      imu_stop();
-  }
-  // write eeprom
-  for(i=address,j=0;i<LAST_EEPROM_OFFSET && i<(address+length);i++,j++)
-    EE_WriteVariable(i,data[j]);
-  // write operation
-  error=ram_write_table(address,length,data);
-  // post-write operation
-
-  return error;
-}
+#include "comm.h"
+#include <math.h>
 
 int main(void)
 {
-  uint8_t inst_packet[1024];
-  uint8_t status_packet[1024];
-  uint8_t data[1024],error,length;
   uint16_t eeprom_data;
-  uint16_t action_period,mm_period;
-  uint8_t i;
+  uint16_t period;
 
   /* initialize EEPROM */
   EE_Init();
@@ -157,88 +42,22 @@ int main(void)
   dyn_slave_set_return_level((uint8_t)eeprom_data);
   // initialize motion manager
   EE_ReadVariable(MM_PERIOD_OFFSET,&eeprom_data);
-  action_period=eeprom_data&0x00FF;
+  period=eeprom_data&0x00FF;
   EE_ReadVariable(MM_PERIOD_OFFSET+1,&eeprom_data);
-  action_period+=((eeprom_data&0x00FF)<<8);
-  mm_period=(action_period*1000000)>>16;
-  manager_init(mm_period);
-  // initialize the action module
-  action_init(action_period);
-  walking_init();
+  period+=((eeprom_data&0x00FF)<<8);
+  manager_init(period);
+  grippers_init();
   // initialize imu
   imu_init();
   // initialize adc
-  adc_init();
-
-  gpio_blink_led(LED_2,1000);
-
-  for(i=0;i<20;i++)
-  {
-    manager_enable_servo(i);
-    manager_set_module(i,MM_WALKING);
-  }
-//  manager_enable();
+  //adc_init();
+  gpio_blink_led(LED_5_R,1000);
+  comm_init();
 
-  walking_start();
+  comm_start();
 
   while(1)                             /* main function does not return */
   {
-    if(dyn_slave_is_packet_ready())// check if a new instruction packet has been received
-    {
-      dyn_slave_get_inst_packet(inst_packet);// get the received packet
-      // check the packet checksum
-      if(dyn_check_checksum(inst_packet)==0xFF)// the incomming packet is okay
-      {
-        // check address
-        if(dyn_get_id(inst_packet)==dyn_slave_get_address())// the packet is addressed to this device
-        {
-          // process the packet
-          switch(dyn_get_instruction(inst_packet))
-          {
-            case DYN_PING: dyn_slave_send_status_packet(DYN_NO_ERROR,0,data);
-                           break;
-            case DYN_READ: error=read_operation(dyn_get_read_address(inst_packet),dyn_get_read_length(inst_packet),data);
-                           if(dyn_slave_get_return_level()!=0)
-                           {
-                             if(error==RAM_SUCCESS)
-                               dyn_slave_send_status_packet(DYN_NO_ERROR,dyn_get_read_length(inst_packet),data);
-                             else
-                               dyn_slave_send_status_packet(DYN_INST_ERROR,0,data);
-                           }
-                           break;
-            case DYN_WRITE: length=dyn_get_write_data(inst_packet,data);
-                            if(length!=DYN_BAD_FORMAT)
-                              error=write_operation(dyn_get_write_address(inst_packet),length,data);
-                            if(dyn_slave_get_return_level()==2)
-                            {
-                              if(error==RAM_SUCCESS || length==DYN_BAD_FORMAT)
-                                dyn_slave_send_status_packet(DYN_NO_ERROR,0,data);
-                              else
-                                dyn_slave_send_status_packet(DYN_INST_ERROR,0,data);
-                            }
-                            break;
-            default:
-                     break;
-          }
-        }
-        else// the packet is addressed to another device
-        {
-          // send the incomming packet to the dynamixel bus
-          if(dyn_master_resend_inst_packet(inst_packet,status_packet)==DYN_NO_ERROR)
-          {
-            // send the answer back to the computer
-            dyn_slave_resend_status_packet(status_packet);
-          }
-          else
-            dyn_slave_reset();// prepare the dynamixel master to accept data
-        }
-      }
-      else
-      {
-        // send a checksum error answer
-        dyn_slave_send_status_packet(DYN_CHECKSUM_ERROR,0,0x00);
-      }
-    }
   }
 }
 
diff --git a/src/comm.c b/src/comm.c
new file mode 100644
index 0000000000000000000000000000000000000000..cb15dacaa11ca638d571f91323305ae3d6c4edbe
--- /dev/null
+++ b/src/comm.c
@@ -0,0 +1,303 @@
+#include "comm.h"
+#include "gpio.h"
+#include "ram.h"
+#include "motion_manager.h"
+#include "action.h"
+#include "walking.h"
+#include "joint_motion.h"
+#include "head_tracking.h"
+#include "grippers.h"
+#include "dynamixel_slave_uart_dma.h"
+#include "dynamixel_master_uart_dma.h"
+#include "imu.h"
+#include "adc_dma.h"
+
+#define      COMM_TIMER                 TIM7
+#define      COMM_TIMER_IRQn            TIM7_IRQn
+#define      COMM_TIMER_IRQHandler      TIM7_IRQHandler
+#define      COMM_TIMER_CLK             RCC_APB1Periph_TIM7
+
+uint8_t read_operation(uint8_t address, uint8_t length, uint8_t *data)
+{
+  uint8_t error;
+
+  // pre-read operation
+  // read operation
+  error=ram_read_table(address,length,data);
+  // post-read operation
+  if(ram_in_range(DARWIN_PUSHBUTTON,DARWIN_PUSHBUTTON,address,length))
+    ram_data[DARWIN_PUSHBUTTON]=0x00;
+
+  return error;
+}
+
+uint8_t write_operation(uint8_t address,uint8_t length, uint8_t *data)
+{
+  uint8_t error=RAM_SUCCESS,byte_value,i,j,do_op=0x00,module;
+  uint16_t word_value;
+
+  // pre-write operation
+  if(ram_in_range(DARWIN_DXL_POWER,DARWIN_DXL_POWER,address,length))
+  {
+    if(data[DARWIN_DXL_POWER-address]&0x01) dyn_master_enable_power();
+    else dyn_master_disable_power();
+  }
+  if(ram_in_range(DARWIN_LED_PANNEL,DARWIN_LED_PANNEL,address,length))
+  {
+    byte_value=data[DARWIN_LED_PANNEL-address];
+    if(byte_value&0x01)
+      gpio_set_led(LED_2);
+    else
+      gpio_clear_led(LED_2);
+    if(byte_value&0x02)
+      gpio_set_led(LED_3);
+    else
+      gpio_clear_led(LED_3);
+    if(byte_value&0x04)
+      gpio_set_led(LED_4);
+    else
+      gpio_clear_led(LED_4);
+    if(byte_value&0x08)
+      gpio_set_led(LED_TX);
+    else
+      gpio_clear_led(LED_TX);
+    if(byte_value&0x10)
+      gpio_set_led(LED_RX);
+    else
+      gpio_clear_led(LED_RX);
+  }
+  if(ram_in_range(DARWIN_MM_PERIOD_L,DARWIN_MM_PERIOD_H,address,length))
+  {
+    word_value=data[DARWIN_MM_PERIOD_L-address]+(data[DARWIN_MM_PERIOD_H-address]<<8);
+    manager_set_period(word_value);
+  }
+  for(i=DARWIN_MM_MODULE_EN0,j=0;i<=DARWIN_MM_MODULE_EN15;i++,j+=2)
+  {
+    do_op=0x01;
+    if(ram_in_range(i,i,address,length))
+    {
+      byte_value=data[i-address];
+      if(byte_value&0x80) manager_enable_servo(j);
+      else manager_disable_servo(j);
+      module=(byte_value&0x70)>>4;
+      manager_set_module(j,module);
+      if(module==MM_JOINTS)
+        joint_motion_load_target_angle(j,current_angles[j]>>9);
+      if(byte_value&0x08) manager_enable_servo(j+1);
+      else manager_disable_servo(j+1);
+      module=byte_value&0x07;
+      manager_set_module(j+1,module);
+      if(module==MM_JOINTS)
+        joint_motion_load_target_angle(j+1,current_angles[j+1]>>9);
+    }
+  }
+  if(do_op)
+  {
+    do_op=0x00;
+    manager_disable_servos();
+  }
+  if(ram_in_range(DARWIN_MM_CONTROL,DARWIN_MM_CONTROL,address,length))
+  {
+    if(data[DARWIN_MM_CONTROL-address]&0x01)
+      manager_enable();
+    else
+      manager_disable();
+    if(data[DARWIN_MM_CONTROL-address]&0x02)
+      manager_enable_balance();
+    else
+      manager_disable_balance();
+  }
+  if(ram_in_range(DARWIN_MM_KNEE_GAIN,DARWIN_MM_KNEE_GAIN,address,length))
+    manager_set_knee_gain(data[DARWIN_MM_CONTROL-address]);
+  if(ram_in_range(DARWIN_MM_ANKLE_PITCH_GAIN,DARWIN_MM_ANKLE_PITCH_GAIN,address,length))
+    manager_set_ankle_pitch_gain(data[DARWIN_MM_CONTROL-address]);
+  if(ram_in_range(DARWIN_MM_HIP_ROLL_GAIN,DARWIN_MM_HIP_ROLL_GAIN,address,length))
+    manager_set_hip_roll_gain(data[DARWIN_MM_CONTROL-address]);
+  if(ram_in_range(DARWIN_MM_ANKLE_ROLL_GAIN,DARWIN_MM_ANKLE_ROLL_GAIN,address,length))
+    manager_set_ankle_roll_gain(data[DARWIN_MM_CONTROL-address]);
+  if(ram_in_range(DARWIN_ACTION_CONTROL,DARWIN_ACTION_CONTROL,address,length))
+  {
+    if(data[DARWIN_ACTION_CONTROL-address]&0x01)
+      action_start_page();
+    if(data[DARWIN_ACTION_CONTROL-address]&0x02)
+      action_stop_page();
+  }
+  if(ram_in_range(DARWIN_ACTION_PAGE,DARWIN_ACTION_PAGE,address,length))// load the page identifier 
+    action_load_page(data[DARWIN_ACTION_PAGE-address]);
+  if(ram_in_range(DARWIN_WALK_CONTROL,DARWIN_WALK_CONTROL,address,length))
+  {
+    if(data[DARWIN_WALK_CONTROL-address]&0x01)
+      walking_start();
+    if(data[DARWIN_WALK_CONTROL-address]&0x02)
+      walking_stop();
+  }
+  for(i=DARWIN_X_OFFSET;i<=DARWIN_MAX_ROT_ACC;i++)
+    if(ram_in_range(i,i,address,length))
+      walking_set_param(i,data[i-address]);
+  if(ram_in_range(DARWIN_IMU_CONTROL,DARWIN_IMU_CONTROL,address,length))
+  {
+    if(data[DARWIN_IMU_CONTROL-address]&0x01)
+      imu_start();
+    else
+      imu_stop();
+    if(data[DARWIN_IMU_CONTROL-address]&0x02)
+      imu_start_gyro_cal();
+  }
+  for(i=DARWIN_MM_SERVO0_CUR_POS_L,j=0;i<=DARWIN_MM_SERVO26_CUR_POS_L;i+=2,j++)
+    if(ram_in_range(i,i+1,address,length))
+      joint_motion_load_target_angle(j,data[i-address]+(data[i+1-address]<<8));
+  for(i=DARWIN_SERVO_0_SPEED,j=0;i<=DARWIN_SERVO_26_SPEED;i++,j++)
+    if(ram_in_range(i,i,address,length))
+      joint_motion_load_target_speed(j,data[i-address]);
+  for(i=DARWIN_SERVO_0_ACCEL,j=0;i<=DARWIN_SERVO_26_ACCEL;i++,j++)
+    if(ram_in_range(i,i,address,length))
+      joint_motion_load_target_accel(j,data[i-address]);
+  if(ram_in_range(DARWIN_JOINTS_CONTROL,DARWIN_JOINTS_CONTROL,address,length))
+  {
+    if(data[DARWIN_JOINTS_CONTROL-address]&0x01)
+      joint_motion_start();
+    if(data[DARWIN_JOINTS_CONTROL-address]&0x02)
+      joint_motion_stop();
+  }
+  if(ram_in_range(DARWIN_HEAD_PAN_L,DARWIN_HEAD_PAN_H,address,length))
+  {
+    word_value=data[DARWIN_HEAD_PAN_L-address]+(data[DARWIN_HEAD_PAN_H-address]<<8);
+    head_tracking_set_target_pan(word_value);
+  }
+  if(ram_in_range(DARWIN_HEAD_TILT_L,DARWIN_HEAD_TILT_H,address,length))
+  {
+    word_value=data[DARWIN_HEAD_TILT_L-address]+(data[DARWIN_HEAD_TILT_H-address]<<8);
+    head_tracking_set_target_tilt(word_value);
+  }
+  if(ram_in_range(DARWIN_HEAD_PAN_P,DARWIN_HEAD_PAN_P,address,length))
+    head_tracking_set_pan_p(data[DARWIN_HEAD_PAN_P-address]);
+  if(ram_in_range(DARWIN_HEAD_PAN_I,DARWIN_HEAD_PAN_I,address,length))
+    head_tracking_set_pan_i(data[DARWIN_HEAD_PAN_I-address]);
+  if(ram_in_range(DARWIN_HEAD_PAN_D,DARWIN_HEAD_PAN_D,address,length))
+    head_tracking_set_pan_d(data[DARWIN_HEAD_PAN_D-address]);
+  if(ram_in_range(DARWIN_HEAD_TILT_P,DARWIN_HEAD_TILT_P,address,length))
+    head_tracking_set_tilt_p(data[DARWIN_HEAD_TILT_P-address]);
+  if(ram_in_range(DARWIN_HEAD_TILT_I,DARWIN_HEAD_TILT_I,address,length))
+    head_tracking_set_tilt_i(data[DARWIN_HEAD_TILT_I-address]);
+  if(ram_in_range(DARWIN_HEAD_TILT_D,DARWIN_HEAD_TILT_D,address,length))
+    head_tracking_set_tilt_d(data[DARWIN_HEAD_TILT_D-address]);
+  if(ram_in_range(DARWIN_HEAD_CONTROL,DARWIN_HEAD_CONTROL,address,length))
+  {
+    if(data[DARWIN_HEAD_CONTROL-address]&0x01)
+      head_tracking_start();
+    else
+      head_tracking_stop();
+  }
+  if(ram_in_range(DARWIN_GRIPPER_CONTROL,DARWIN_GRIPPER_CONTROL,address,length))
+  {
+    if(data[DARWIN_GRIPPER_CONTROL-address]&0x01)
+      grippers_open(LEFT_GRIPPER);
+    else if(data[DARWIN_GRIPPER_CONTROL-address]&0x02)
+      grippers_close(LEFT_GRIPPER);
+    else if(data[DARWIN_GRIPPER_CONTROL-address]&0x04)
+      grippers_open(RIGHT_GRIPPER);
+    else if(data[DARWIN_GRIPPER_CONTROL-address]&0x08)
+      grippers_close(RIGHT_GRIPPER);
+  }
+  // write eeprom
+  for(i=address,j=0;i<LAST_EEPROM_OFFSET && i<(address+length);i++,j++)
+    EE_WriteVariable(i,data[j]);
+  // write operation
+  // error=ram_write_table(address,length,data);
+  // post-write operation
+
+  return error;
+}
+
+void COMM_TIMER_IRQHandler(void)
+{
+  uint8_t inst_packet[2*MAX_DATA_LENGTH];
+  uint8_t status_packet[2*MAX_DATA_LENGTH];
+  uint8_t data[2*MAX_DATA_LENGTH],error,length;
+
+  TIM_ClearITPendingBit(COMM_TIMER,TIM_IT_Update);
+
+  if(dyn_slave_is_packet_ready())// check if a new instruction packet has been received
+  {
+    dyn_slave_get_inst_packet(inst_packet);// get the received packet
+    // check the packet checksum
+    if(dyn_check_checksum(inst_packet)==0xFF)// the incomming packet is okay
+    {
+      // check address
+      if(dyn_get_id(inst_packet)==dyn_slave_get_address())// the packet is addressed to this device
+      {
+        // process the packet
+        switch(dyn_get_instruction(inst_packet))
+        {
+          case DYN_PING: dyn_slave_send_status_packet(DYN_NO_ERROR,0,data);
+                         break;
+          case DYN_READ: error=read_operation(dyn_get_read_address(inst_packet),dyn_get_read_length(inst_packet),data);
+                         if(dyn_slave_get_return_level()!=0)
+                         {
+                           if(error==RAM_SUCCESS)
+                             dyn_slave_send_status_packet(DYN_NO_ERROR,dyn_get_read_length(inst_packet),data);
+                           else
+                             dyn_slave_send_status_packet(DYN_INST_ERROR,0,data);
+                         }
+                         break;
+          case DYN_WRITE: length=dyn_get_write_data(inst_packet,data);
+                          if(length!=DYN_BAD_FORMAT)
+                            error=write_operation(dyn_get_write_address(inst_packet),length,data);
+                          if(dyn_slave_get_return_level()==2)
+                          {
+                            if(error==RAM_SUCCESS || length==DYN_BAD_FORMAT)
+                              dyn_slave_send_status_packet(DYN_NO_ERROR,0,data);
+                            else
+                              dyn_slave_send_status_packet(DYN_INST_ERROR,0,data);
+                          }
+                          break;
+          default:
+                   break;
+        }
+      }
+      else// the packet is addressed to another device
+      {
+        // send the incomming packet to the dynamixel bus
+        if(dyn_master_resend_inst_packet(inst_packet,status_packet)==DYN_NO_ERROR)
+        {
+          // send the answer back to the computer
+          dyn_slave_resend_status_packet(status_packet);
+        }
+        else
+          dyn_slave_reset();// prepare the dynamixel master to accept data
+      }
+    }
+    else
+    {
+      // send a checksum error answer
+      dyn_slave_send_status_packet(DYN_CHECKSUM_ERROR,0,0x00);
+    }
+  }
+}
+
+void comm_init(void)
+{
+  TIM_TimeBaseInitTypeDef  TIM_TimeBaseStructure;
+  NVIC_InitTypeDef NVIC_InitStructure;
+
+  RCC_APB1PeriphClockCmd(COMM_TIMER_CLK,ENABLE);
+
+  TIM_TimeBaseStructure.TIM_Period = 1000;
+  TIM_TimeBaseStructure.TIM_Prescaler = 72;
+  TIM_TimeBaseStructure.TIM_ClockDivision = 0;
+  TIM_TimeBaseStructure.TIM_CounterMode = TIM_CounterMode_Up;
+  TIM_TimeBaseInit(COMM_TIMER,&TIM_TimeBaseStructure);
+
+  NVIC_InitStructure.NVIC_IRQChannel = COMM_TIMER_IRQn;
+  NVIC_InitStructure.NVIC_IRQChannelPreemptionPriority = 1;
+  NVIC_InitStructure.NVIC_IRQChannelSubPriority = 3;
+  NVIC_InitStructure.NVIC_IRQChannelCmd = ENABLE;
+  NVIC_Init(&NVIC_InitStructure);
+}
+
+void comm_start(void)
+{
+  TIM_Cmd(COMM_TIMER, ENABLE);
+  TIM_ITConfig(COMM_TIMER, TIM_IT_Update, ENABLE);
+}
+
diff --git a/src/dynamixel_master_uart_dma.c b/src/dynamixel_master_uart_dma.c
index d384c7fe76ed5a0d14b2a8c8230e645110840f3d..936781e745d7bdda06b85cae10e1e8cdf68816a0 100755
--- a/src/dynamixel_master_uart_dma.c
+++ b/src/dynamixel_master_uart_dma.c
@@ -1,6 +1,7 @@
 #include "dynamixel_master_uart_dma.h"
+#include "motion_manager.h"
 #include "time.h"
-#include "gpio.h"
+#include "ram.h"
 
 #define     USART                    USART1
 #define     USART_CLK                RCC_APB2Periph_USART1
@@ -59,6 +60,7 @@
 #define     MAX_BUFFER_LEN           1024
 
 // private variables
+uint8_t dyn_master_version;
 uint16_t dyn_master_timeout;// answer reception timeout
 // input buffer
 uint8_t dyn_master_rx_buffer[MAX_BUFFER_LEN];
@@ -73,6 +75,9 @@ volatile uint8_t dyn_master_sending_packet;
 uint8_t dyn_master_no_answer;
 // power enabled variable
 uint8_t dyn_master_power_enabled;
+// receiving a bulk read backet
+uint8_t dyn_bulk_read_packet;
+uint8_t dyn_bulk_read_length;
 // DMA initialization data structures
 DMA_InitTypeDef  DMA_TX_InitStructure;
 DMA_InitTypeDef  DMA_RX_InitStructure;
@@ -86,12 +91,15 @@ void dyn_master_reset(void)
   /* wait until the transaction ends */
   DMA_ClearFlag(USART_RX_DMA_FLAG_GLIF);
   DMA_ClearITPendingBit(USART_RX_DMA_FLAG_GLIF);
+  /* clear any pending data */
+  USART_ReceiveData(USART);
   dyn_master_receiving_header=0x01;
   dyn_master_packet_ready=0x00;
 }
 
 void dyn_master_send(void)
 {
+  dyn_master_version=1;
   // wait until any previous transmission ends
   while(dyn_master_sending_packet);
   // set the DMA transfer
@@ -102,6 +110,19 @@ void dyn_master_send(void)
   dyn_master_sending_packet=0x01;
 }
 
+void dyn2_master_send(void)
+{
+  dyn_master_version=2;
+  // wait until any previous transmission ends
+  while(dyn_master_sending_packet);
+  // set the DMA transfer
+  DMA_SetCurrDataCounter(USART_TX_DMA_CHANNEL,dyn2_get_length(dyn_master_tx_buffer)+7);
+  DMA_Cmd(USART_TX_DMA_CHANNEL,ENABLE);
+  USART_ClearFlag(USART,USART_FLAG_TC);
+  USART_DMACmd(USART, USART_DMAReq_Tx, ENABLE);
+  dyn_master_sending_packet=0x01;
+}
+
 uint8_t dyn_master_receive(void)
 {
   int16_t timeout_left=dyn_master_timeout;
@@ -122,7 +143,36 @@ uint8_t dyn_master_receive(void)
   if(dyn_check_checksum(dyn_master_rx_buffer)==0xFF)
     return dyn_get_status_error(dyn_master_rx_buffer);
   else
+  {
+    dyn_master_reset();
     return DYN_CHECKSUM_ERROR;
+  }
+}
+
+uint8_t dyn2_master_receive(void)
+{
+  int16_t timeout_left=dyn_master_timeout;
+
+  // wait for the status packet
+  while(!dyn_master_packet_ready)
+  {
+    delay_ms(1);
+    timeout_left-=1;
+    if(timeout_left<=0)
+    {
+      dyn_master_reset();
+      return DYN_TIMEOUT;
+    }
+  }
+  dyn_master_packet_ready=0x00;
+  // check the input packet checksum
+  if(dyn2_check_status_crc(dyn_master_rx_buffer)==0x01)
+    return dyn2_get_status_error(dyn_master_rx_buffer);
+  else
+  {
+    dyn_master_reset();
+    return DYN_CHECKSUM_ERROR;
+  }
 }
 
 void dyn_master_enable_tx(void)
@@ -175,6 +225,44 @@ uint8_t dyn_master_read(uint8_t id,uint8_t address,uint8_t length,uint8_t *data)
     return error;
 }
 
+uint8_t dyn2_master_read(uint8_t id,uint16_t address,uint16_t length,uint8_t *data)
+{
+  uint8_t error;
+
+  // wait for the transmission to end
+  while(dyn_master_sending_packet);
+  // generate the read packet for the desired device
+  dyn2_init_instruction_packet(dyn_master_tx_buffer);
+  // set the ping instruction
+  dyn2_set_instruction(dyn_master_tx_buffer,DYN_READ);
+  // set the device id
+  if((error=dyn2_set_id(dyn_master_tx_buffer,id))==DYN_SUCCESS)
+  {
+    // set the length to read
+    dyn2_set_read_length(dyn_master_tx_buffer,length);
+    // sert the start address to start reading
+    dyn2_set_read_address(dyn_master_tx_buffer,address);
+    // set the checksum
+    dyn2_set_inst_crc(dyn_master_tx_buffer);
+    // enable transmission
+    dyn_master_enable_tx();
+    // send the data
+    dyn2_master_send();
+    // wait for the transmission to end
+    while(dyn_master_sending_packet);
+    dyn_master_enable_rx();
+    // wait for the replay within the given timeout
+    error=dyn2_master_receive();
+    if(error==DYN_NO_ERROR)
+      dyn2_get_status_data(dyn_master_rx_buffer,data);// read the data from the status packet
+    else
+      dyn_master_reset();
+    return error;
+  }
+  else
+    return error;
+}
+
 uint8_t dyn_master_write(uint8_t id, uint8_t address, uint8_t length, uint8_t *data)
 {
   uint8_t error;
@@ -211,6 +299,42 @@ uint8_t dyn_master_write(uint8_t id, uint8_t address, uint8_t length, uint8_t *d
     return error;
 }
 
+uint8_t dyn2_master_write(uint8_t id, uint16_t address, uint16_t length, uint8_t *data)
+{
+  uint8_t error;
+
+  // wait for the transmission to end
+  while(dyn_master_sending_packet);
+  // generate the read packet for the desired device
+  dyn2_init_instruction_packet(dyn_master_tx_buffer);
+  // set the ping instruction
+  dyn2_set_instruction(dyn_master_tx_buffer,DYN_WRITE);
+  // set the device id
+  if((error=dyn2_set_id(dyn_master_tx_buffer,id))==DYN_SUCCESS)
+  {
+    // sert the start address to start reading
+    dyn2_set_read_address(dyn_master_tx_buffer,address);
+    // set the data to write and its length 
+    dyn2_set_write_data(dyn_master_tx_buffer,length,data);
+    // set the checksum
+    dyn2_set_inst_crc(dyn_master_tx_buffer);
+    // enable transmission
+    dyn_master_enable_tx();
+    // send the data
+    dyn2_master_send();
+    // wait for the transmission to end
+    while(dyn_master_sending_packet);
+    dyn_master_enable_rx();
+    // wait for the replay within the given timeout
+    error=dyn2_master_receive();
+    if(error!=DYN_NO_ERROR)
+      dyn_master_reset();
+    return error;
+  }
+  else
+    return error;
+}
+
 // interrupt handlers
 void USART_IRQHandler(void)
 {
@@ -223,14 +347,32 @@ void USART_IRQHandler(void)
       dyn_master_no_answer=0x00;
     else
     {
-      // set up the DMA RX transaction
-      DMA_RX_InitStructure.DMA_BufferSize = 4;
-      DMA_RX_InitStructure.DMA_MemoryBaseAddr = (uint32_t)dyn_master_rx_buffer;
-      DMA_Init(USART_RX_DMA_CHANNEL,&DMA_RX_InitStructure);
-      DMA_ITConfig(USART_RX_DMA_CHANNEL,DMA_IT_TC,ENABLE);
-      DMA_Cmd(USART_RX_DMA_CHANNEL,ENABLE);
-      USART_DMACmd(USART, USART_DMAReq_Rx, ENABLE);
-      dyn_master_receiving_header=0x01;
+      if(dyn_bulk_read_packet==0x00)
+      {
+        // set up the DMA RX transaction
+        if(dyn_master_version==1)
+          DMA_RX_InitStructure.DMA_BufferSize = 4;
+        else
+          DMA_RX_InitStructure.DMA_BufferSize = 7;
+        DMA_RX_InitStructure.DMA_MemoryBaseAddr = (uint32_t)dyn_master_rx_buffer;
+        DMA_Init(USART_RX_DMA_CHANNEL,&DMA_RX_InitStructure);
+        DMA_ITConfig(USART_RX_DMA_CHANNEL,DMA_IT_TC,ENABLE);
+        DMA_Cmd(USART_RX_DMA_CHANNEL,ENABLE);
+        USART_DMACmd(USART, USART_DMAReq_Rx, ENABLE);
+        dyn_master_receiving_header=0x01;
+      }
+      else
+      {
+        // set up the DMA RX transaction
+        DMA_RX_InitStructure.DMA_BufferSize = dyn_bulk_read_length;
+        DMA_RX_InitStructure.DMA_MemoryBaseAddr = (uint32_t)dyn_master_rx_buffer;
+        DMA_Init(USART_RX_DMA_CHANNEL,&DMA_RX_InitStructure);
+        DMA_ITConfig(USART_RX_DMA_CHANNEL,DMA_IT_TC,ENABLE);
+        DMA_Cmd(USART_RX_DMA_CHANNEL,ENABLE);
+        USART_DMACmd(USART, USART_DMAReq_Rx, ENABLE);
+        dyn_master_receiving_header=0x00;
+        dyn_bulk_read_packet=0x00;
+      }
     }
   }
 }
@@ -248,8 +390,16 @@ void USART_DMA_RX_IRQHandler(void)
   if(dyn_master_receiving_header==0x01)
   {
     DMA_Cmd(USART_RX_DMA_CHANNEL,DISABLE);
-    DMA_RX_InitStructure.DMA_BufferSize = dyn_get_length(dyn_master_rx_buffer);
-    DMA_RX_InitStructure.DMA_MemoryBaseAddr = (uint32_t)&dyn_master_rx_buffer[4];
+    if(dyn_master_version==1)
+    {
+      DMA_RX_InitStructure.DMA_BufferSize = dyn_get_length(dyn_master_rx_buffer);
+      DMA_RX_InitStructure.DMA_MemoryBaseAddr = (uint32_t)&dyn_master_rx_buffer[4];
+    }
+    else
+    {
+      DMA_RX_InitStructure.DMA_BufferSize = dyn2_get_length(dyn_master_rx_buffer);
+      DMA_RX_InitStructure.DMA_MemoryBaseAddr = (uint32_t)&dyn_master_rx_buffer[7];
+    }
     DMA_Init(USART_RX_DMA_CHANNEL,&DMA_RX_InitStructure);
     DMA_Cmd(USART_RX_DMA_CHANNEL,ENABLE);
     USART_DMACmd(USART, USART_DMAReq_Rx, ENABLE);
@@ -318,6 +468,9 @@ void dyn_master_init(void)
   dyn_master_sending_packet=0x00;
   dyn_master_receiving_header=0x01;
   dyn_master_no_answer=0x00;
+  dyn_bulk_read_packet=0x00;
+  dyn_bulk_read_length=0x00;
+  dyn_master_version=1;
 
   USART_DeInit(USART);
   USART_StructInit(&USART_InitStructure);
@@ -330,7 +483,7 @@ void dyn_master_init(void)
   USART_InitStructure.USART_Mode = USART_Mode_Rx | USART_Mode_Tx;
   USART_Init(USART, &USART_InitStructure);
   NVIC_InitStructure.NVIC_IRQChannel = USART_IRQn;
-  NVIC_InitStructure.NVIC_IRQChannelPreemptionPriority = 0;
+  NVIC_InitStructure.NVIC_IRQChannelPreemptionPriority = 2;
   NVIC_InitStructure.NVIC_IRQChannelSubPriority = 2;
   NVIC_InitStructure.NVIC_IRQChannelCmd = ENABLE;
   NVIC_Init(&NVIC_InitStructure);
@@ -353,7 +506,7 @@ void dyn_master_init(void)
   DMA_Init(USART_TX_DMA_CHANNEL,&DMA_TX_InitStructure);
   /* initialize DMA interrupts */
   NVIC_InitStructure.NVIC_IRQChannel = USART_DMA_TX_IRQn;
-  NVIC_InitStructure.NVIC_IRQChannelPreemptionPriority = 0;
+  NVIC_InitStructure.NVIC_IRQChannelPreemptionPriority = 2;
   NVIC_InitStructure.NVIC_IRQChannelSubPriority = 0;
   NVIC_InitStructure.NVIC_IRQChannelCmd = ENABLE;
   NVIC_Init(&NVIC_InitStructure);
@@ -375,7 +528,7 @@ void dyn_master_init(void)
   DMA_Init(USART_RX_DMA_CHANNEL,&DMA_RX_InitStructure);
   /* initialize DMA interrupts */
   NVIC_InitStructure.NVIC_IRQChannel = USART_DMA_RX_IRQn;
-  NVIC_InitStructure.NVIC_IRQChannelPreemptionPriority = 0;
+  NVIC_InitStructure.NVIC_IRQChannelPreemptionPriority = 2;
   NVIC_InitStructure.NVIC_IRQChannelSubPriority = 1;
   NVIC_InitStructure.NVIC_IRQChannelCmd = ENABLE;
   NVIC_Init(&NVIC_InitStructure);
@@ -393,11 +546,6 @@ void dyn_master_init(void)
   dyn_master_disable_power();
 }
 
-void dyn_master_flush(void)
-{
-  // flush only the reception buffer to avoid interrupting a sync_write command
-}
-
 void dyn_master_set_timeout(uint16_t timeout_ms)
 {
   // save the desired timeout value
@@ -408,6 +556,8 @@ void dyn_master_scan(uint8_t *num,uint8_t *ids)
 {
   uint8_t i;
 
+  for(i=0;i<MANAGER_MAX_NUM_SERVOS;i++)
+    ids[0]=0;
   *num=0;
   for(i=0;i<254;i++)
   {
@@ -419,6 +569,23 @@ void dyn_master_scan(uint8_t *num,uint8_t *ids)
   }
 }
 
+void dyn2_master_scan(uint8_t *num,uint8_t *ids)
+{
+  uint8_t i;
+
+  for(i=0;i<MANAGER_MAX_NUM_SERVOS;i++)
+    ids[0]=0;
+  *num=0;
+  for(i=0;i<254;i++)
+  {
+    if(dyn2_master_ping(i)==DYN_NO_ERROR)// the device exists
+    {
+      ids[*num]=i;
+      (*num)++;
+    }
+  }
+}
+
 inline void dyn_master_enable_power(void)
 {
   GPIO_SetBits(POWER_GPIO_PORT,POWER_PIN);
@@ -464,11 +631,44 @@ uint8_t dyn_master_ping(uint8_t id)
     return error;
 }
 
+uint8_t dyn2_master_ping(uint8_t id)
+{
+  uint8_t error;
+
+  // generate the ping packet for the desired device
+  dyn2_init_instruction_packet(dyn_master_tx_buffer);
+  // set the ping instruction
+  dyn2_set_instruction(dyn_master_tx_buffer,DYN_PING);
+  // set the device id
+  if((error=dyn2_set_id(dyn_master_tx_buffer,id))==DYN_SUCCESS)
+  {
+    // set the checksum
+    dyn2_set_inst_crc(dyn_master_tx_buffer);
+    // enable transmission
+    dyn_master_enable_tx();
+    // send the data
+    dyn2_master_send();
+    // wait for the transmission to end
+    while (dyn_master_sending_packet==0x01);
+    dyn_master_enable_rx();
+    // wait for the replay within the given timeout
+    error=dyn2_master_receive();
+    return error;
+  }
+  else
+    return error;
+}
+
 uint8_t dyn_master_read_byte(uint8_t id,uint8_t address,uint8_t *data)
 {
   return dyn_master_read(id,address,1,data);
 }
 
+uint8_t dyn2_master_read_byte(uint8_t id,uint16_t address,uint8_t *data)
+{
+  return dyn2_master_read(id,address,1,data);
+}
+
 uint8_t dyn_master_read_word(uint8_t id,uint8_t address,uint16_t *data)
 {
   uint8_t value[2];
@@ -481,16 +681,38 @@ uint8_t dyn_master_read_word(uint8_t id,uint8_t address,uint16_t *data)
   return error;
 }
 
+uint8_t dyn2_master_read_word(uint8_t id,uint16_t address,uint16_t *data)
+{
+  uint8_t value[2];
+  uint8_t error;
+
+  error=dyn2_master_read(id,address,2,value);
+  if(error==DYN_NO_ERROR)
+    (*data)=value[0]+value[1]*256;
+
+  return error;
+}
+
 uint8_t dyn_master_read_table(uint8_t id,uint8_t address,uint8_t length,uint8_t *data)
 {
   return dyn_master_read(id,address,length,data);
 }
 
+uint8_t dyn2_master_read_table(uint8_t id,uint16_t address,uint16_t length,uint8_t *data)
+{
+  return dyn2_master_read(id,address,length,data);
+}
+
 uint8_t dyn_master_write_byte(uint8_t id, uint8_t address, uint8_t data)
 {
   return dyn_master_write(id,address,1,&data);
 }
 
+uint8_t dyn2_master_write_byte(uint8_t id, uint16_t address, uint8_t data)
+{
+  return dyn2_master_write(id,address,1,&data);
+}
+
 uint8_t dyn_master_write_word(uint8_t id, uint8_t address, uint16_t data)
 {
   uint8_t value[2];
@@ -500,11 +722,25 @@ uint8_t dyn_master_write_word(uint8_t id, uint8_t address, uint16_t data)
   return dyn_master_write(id,address,2,value);
 }
 
+uint8_t dyn2_master_write_word(uint8_t id, uint16_t address, uint16_t data)
+{
+  uint8_t value[2];
+
+  value[0]=data%256;
+  value[1]=data/256;
+  return dyn2_master_write(id,address,2,value);
+}
+
 uint8_t dyn_master_write_table(uint8_t id, uint8_t address, uint8_t length, uint8_t *data)
 {
   return dyn_master_write(id,address,length,data);
 }
 
+uint8_t dyn2_master_write_table(uint8_t id, uint16_t address, uint16_t length, uint8_t *data)
+{
+  return dyn2_master_write(id,address,length,data);
+}
+
 uint8_t dyn_master_reg_write(uint8_t id, uint8_t address, uint8_t length, uint8_t *data)
 {
   return DYN_SUCCESS;
@@ -542,6 +778,36 @@ void dyn_master_sync_write(uint8_t num,uint8_t *ids,uint8_t address, uint8_t len
   // wait for the transmission to end
 }
 
+void dyn2_master_sync_write(uint8_t num,uint8_t *ids,uint16_t address, uint16_t length, uint8_t *data)
+{
+  uint8_t i;
+
+  // wait for the transmission to end
+  while(dyn_master_sending_packet);
+  dyn_master_no_answer=0x01;
+  // generate the sync write packet 
+  dyn2_init_instruction_packet(dyn_master_tx_buffer);
+  // set the ping instruction
+  dyn2_set_instruction(dyn_master_tx_buffer,DYN_SYNC_WRITE);
+  // set the start address
+  dyn2_set_write_address(dyn_master_tx_buffer,address);
+  // set the data length
+  dyn2_set_sync_write_length(dyn_master_tx_buffer,length);
+  // load the data for each device
+  for(i=0;i<num;i++)
+    dyn2_set_sync_write_data(dyn_master_tx_buffer,ids[i],&data[i*length]);
+  // set the checksum
+  dyn2_set_inst_crc(dyn_master_tx_buffer);
+  // enable transmission
+  dyn_master_enable_tx();
+  // send the data
+  for(i=0;i<27;i++)
+    ram_data[DARWIN_SERVO_0_SPEED+i]=dyn_master_tx_buffer[i];
+
+  dyn2_master_send();
+  // wait for the transmission to end
+}
+
 // repeater functions
 uint8_t dyn_master_resend_inst_packet(uint8_t *inst_packet, uint8_t *status_packet)
 {
@@ -569,3 +835,48 @@ uint8_t dyn_master_resend_inst_packet(uint8_t *inst_packet, uint8_t *status_pack
   return error;
 }
 
+uint8_t dyn_master_bulk_read(uint8_t num,TBulkData *info)
+{
+  uint8_t i,error;
+
+  // wait for the transmission to end
+  while(dyn_master_sending_packet);
+  dyn_bulk_read_packet=0x01;
+  dyn_bulk_read_length=0x00;
+  // generate the sync write packet 
+  dyn_init_instruction_packet(dyn_master_tx_buffer);
+  // set the ping instruction
+  dyn_set_instruction(dyn_master_tx_buffer,DYN_BULK_READ);
+  // set the start address
+  dyn_set_write_address(dyn_master_tx_buffer,0x00);
+  // load the data for each device
+  for(i=0;i<num;i++)
+  {
+    dyn_set_bulk_read_data(dyn_master_tx_buffer,info[i].id,info[i].length,info[i].address);
+    dyn_bulk_read_length+=info[i].length+6;
+  }
+  // set the checksum
+  dyn_set_checksum(dyn_master_tx_buffer);
+  // enable transmission
+  dyn_master_enable_tx();
+  // send the data
+  dyn_master_send();
+  // wait for the transmission to end
+  while(dyn_master_sending_packet);
+  dyn_master_enable_rx();
+  // wait for the replay within the given timeout
+  error=dyn_master_receive();
+  if(error!=DYN_NO_ERROR)
+  {
+    dyn_master_reset();
+    return error;
+  }
+  for(i=0;i<num;i++)
+  {
+    error=dyn_get_bulk_read_data(dyn_master_rx_buffer,dyn_bulk_read_length,info[i].id,info[i].data);
+    if(error!=DYN_SUCCESS)
+      return error;
+  }
+
+  return DYN_SUCCESS;
+}
diff --git a/src/dynamixel_slave_uart_dma.c b/src/dynamixel_slave_uart_dma.c
index 268b331fe102a8c55c1ff38f3f6212a5b9637412..6363f7e3e9e0f29a2b6e6b7cdd0aed07b0b4bfd0 100755
--- a/src/dynamixel_slave_uart_dma.c
+++ b/src/dynamixel_slave_uart_dma.c
@@ -91,14 +91,25 @@ void DYN_SLAVE_DMA_RX_IRQHandler(void)
   if(dyn_slave_receiving_header==0x01)
   {
     DMA_Cmd(DYN_SLAVE_RX_DMA_CHANNEL,DISABLE);
-    DYN_SLAVE_DMA_RX_InitStructure.DMA_BufferSize = dyn_get_length(dyn_slave_rx_buffer);
-    DYN_SLAVE_DMA_RX_InitStructure.DMA_MemoryBaseAddr = (uint32_t)&dyn_slave_rx_buffer[4];
-    DMA_Init(DYN_SLAVE_RX_DMA_CHANNEL,&DYN_SLAVE_DMA_RX_InitStructure);
-    DMA_Cmd(DYN_SLAVE_RX_DMA_CHANNEL,ENABLE);
-    USART_DMACmd(DYN_SLAVE, USART_DMAReq_Rx, ENABLE);
-    dyn_slave_receiving_header=0x00;
-    DMA_ClearFlag(DYN_SLAVE_RX_DMA_FLAG_GLIF);
-    DMA_ClearITPendingBit(DYN_SLAVE_RX_DMA_FLAG_GLIF);
+    if(dyn_get_length(dyn_slave_rx_buffer)==0x00)
+    {
+      DMA_Cmd(DYN_SLAVE_RX_DMA_CHANNEL,DISABLE);
+      DMA_ClearFlag(DYN_SLAVE_RX_DMA_FLAG_GLIF);
+      DMA_ClearITPendingBit(DYN_SLAVE_RX_DMA_FLAG_GLIF);
+      DMA_ITConfig(DYN_SLAVE_RX_DMA_CHANNEL,DMA_IT_TC,DISABLE);
+      dyn_slave_packet_ready=0x01;
+    }
+    else
+    {
+      DYN_SLAVE_DMA_RX_InitStructure.DMA_BufferSize = dyn_get_length(dyn_slave_rx_buffer);
+      DYN_SLAVE_DMA_RX_InitStructure.DMA_MemoryBaseAddr = (uint32_t)&dyn_slave_rx_buffer[4];
+      DMA_Init(DYN_SLAVE_RX_DMA_CHANNEL,&DYN_SLAVE_DMA_RX_InitStructure);
+      DMA_Cmd(DYN_SLAVE_RX_DMA_CHANNEL,ENABLE);
+      USART_DMACmd(DYN_SLAVE, USART_DMAReq_Rx, ENABLE);
+      dyn_slave_receiving_header=0x00;
+      DMA_ClearFlag(DYN_SLAVE_RX_DMA_FLAG_GLIF);
+      DMA_ClearITPendingBit(DYN_SLAVE_RX_DMA_FLAG_GLIF);
+    }
   }
   else
   {
diff --git a/src/eeprom.c b/src/eeprom.c
index 96dff4b16d429620d008653df5e6c0f946ffb615..0477a437945e2a91869fe6f08d94c81c5c4ec9c9 100755
--- a/src/eeprom.c
+++ b/src/eeprom.c
@@ -30,12 +30,12 @@
 /* Private define ------------------------------------------------------------*/
 #define    EEPROM_SIZE               0x09
 #define    DEFAULT_DEVICE_MODEL      0x7300
-#define    DEFAULT_FIRMWARE_VERSION  0x13
-#define    DEFAULT_DEVICE_ID         0xC0
-#define    DEFAULT_BAUDRATE          0x01
-#define    DEFAULT_RETURN_DELAY      0x00
+#define    DEFAULT_FIRMWARE_VERSION  0x0013
+#define    DEFAULT_DEVICE_ID         0x00C0
+#define    DEFAULT_BAUDRATE          0x0001
+#define    DEFAULT_RETURN_DELAY      0x0000
 #define    DEFAULT_MM_PERIOD         0x01FF
-#define    DEFAULT_RETURN_LEVEL      0x02
+#define    DEFAULT_RETURN_LEVEL      0x0002
 /* Private macro -------------------------------------------------------------*/
 /* Private variables ---------------------------------------------------------*/
 
diff --git a/src/gpio.c b/src/gpio.c
index 0c1c27795bbc24df2d6c4fa6106a1b93d2a3396a..aaa06b20bd9a96155152ac3b5f79fc854e52c6c8 100755
--- a/src/gpio.c
+++ b/src/gpio.c
@@ -1,4 +1,5 @@
 #include "gpio.h"
+#include "ram.h"
 
 #define LED_TX_GPIO_CLK             RCC_APB2Periph_GPIOC
 #define LED_TX_PIN                  GPIO_Pin_13                
@@ -332,8 +333,8 @@ void gpio_init(void)
 
   // initialize the timer interrupts
   NVIC_InitStructure.NVIC_IRQChannel = GPO_TIMER1_IRQn;
-  NVIC_InitStructure.NVIC_IRQChannelPreemptionPriority = 0;
-  NVIC_InitStructure.NVIC_IRQChannelSubPriority = 0;
+  NVIC_InitStructure.NVIC_IRQChannelPreemptionPriority = 3;
+  NVIC_InitStructure.NVIC_IRQChannelSubPriority = 1;
   NVIC_InitStructure.NVIC_IRQChannelCmd = ENABLE;
   NVIC_Init(&NVIC_InitStructure);
 
@@ -349,8 +350,8 @@ void gpio_init(void)
 
   // initialize the timer interrupts
   NVIC_InitStructure.NVIC_IRQChannel = GPO_TIMER2_IRQn;
-  NVIC_InitStructure.NVIC_IRQChannelPreemptionPriority = 0;
-  NVIC_InitStructure.NVIC_IRQChannelSubPriority = 1;
+  NVIC_InitStructure.NVIC_IRQChannelPreemptionPriority = 3;
+  NVIC_InitStructure.NVIC_IRQChannelSubPriority = 2;
   NVIC_InitStructure.NVIC_IRQChannelCmd = ENABLE;
   NVIC_Init(&NVIC_InitStructure);
 
@@ -366,8 +367,8 @@ void gpio_init(void)
 
   // initialize the timer interrupts
   NVIC_InitStructure.NVIC_IRQChannel = GPO_TIMER3_IRQn;
-  NVIC_InitStructure.NVIC_IRQChannelPreemptionPriority = 0;
-  NVIC_InitStructure.NVIC_IRQChannelSubPriority = 2;
+  NVIC_InitStructure.NVIC_IRQChannelPreemptionPriority = 3;
+  NVIC_InitStructure.NVIC_IRQChannelSubPriority = 3;
   NVIC_InitStructure.NVIC_IRQChannelCmd = ENABLE;
   NVIC_Init(&NVIC_InitStructure);
 
@@ -389,7 +390,7 @@ void gpio_init(void)
   EXTI_Init(&EXTI_InitStructure);
 
   NVIC_InitStructure.NVIC_IRQChannel = GPI_EXTI1_IRQn;
-  NVIC_InitStructure.NVIC_IRQChannelPreemptionPriority = 0;
+  NVIC_InitStructure.NVIC_IRQChannelPreemptionPriority = 2;
   NVIC_InitStructure.NVIC_IRQChannelSubPriority = 3;
   NVIC_InitStructure.NVIC_IRQChannelCmd = ENABLE;
   NVIC_Init(&NVIC_InitStructure);
@@ -414,18 +415,23 @@ void gpio_set_led(led_t led_id)
   {
     case LED_TX:
       GPIO_ResetBits(LED_TX_GPIO_PORT,LED_TX_PIN);
+      ram_set_bit(DARWIN_LED_PANNEL,3);
       break;
     case LED_RX:
       GPIO_ResetBits(LED_RX_GPIO_PORT,LED_RX_PIN);
+      ram_set_bit(DARWIN_LED_PANNEL,4);
       break;
     case LED_2:
       GPIO_ResetBits(LED_2_GPIO_PORT,LED_2_PIN);
+      ram_set_bit(DARWIN_LED_PANNEL,0);
       break;
     case LED_3:
       GPIO_ResetBits(LED_3_GPIO_PORT,LED_3_PIN);
+      ram_set_bit(DARWIN_LED_PANNEL,1);
       break;
     case LED_4:
       GPIO_ResetBits(LED_4_GPIO_PORT,LED_4_PIN);
+      ram_set_bit(DARWIN_LED_PANNEL,2);
       break;
     case LED_5_R:
       GPIO_ResetBits(LED_5_R_GPIO_PORT,LED_5_R_PIN);
@@ -454,18 +460,23 @@ void gpio_clear_led(led_t led_id)
   {
     case LED_TX:
       GPIO_SetBits(LED_TX_GPIO_PORT,LED_TX_PIN);
+      ram_clear_bit(DARWIN_LED_PANNEL,3);
       break;
     case LED_RX:
       GPIO_SetBits(LED_RX_GPIO_PORT,LED_RX_PIN);
+      ram_clear_bit(DARWIN_LED_PANNEL,4);
       break;
     case LED_2:
       GPIO_SetBits(LED_2_GPIO_PORT,LED_2_PIN);
+      ram_clear_bit(DARWIN_LED_PANNEL,0);
       break;
     case LED_3:
       GPIO_SetBits(LED_3_GPIO_PORT,LED_3_PIN);
+      ram_clear_bit(DARWIN_LED_PANNEL,1);
       break;
     case LED_4:
       GPIO_SetBits(LED_4_GPIO_PORT,LED_4_PIN);
+      ram_clear_bit(DARWIN_LED_PANNEL,2);
       break;
     case LED_5_R:
       GPIO_SetBits(LED_5_R_GPIO_PORT,LED_5_R_PIN);
diff --git a/src/grippers.c b/src/grippers.c
new file mode 100644
index 0000000000000000000000000000000000000000..f7fb14b8c593370d12393ddc6851f11680bc44c2
--- /dev/null
+++ b/src/grippers.c
@@ -0,0 +1,48 @@
+#include "grippers.h"
+#include "motion_manager.h"
+#include "ram.h"
+
+const uint16_t left_close_value=-120;
+const uint16_t left_open_value=-20;
+const uint16_t right_close_value=-140;
+const uint16_t right_open_value=-40;
+
+/* public functions */
+void grippers_init(void)
+{
+  ram_data[DARWIN_GRIPPER_CONTROL]=0x00;
+}
+
+void grippers_set_force(grippers_t gripper,uint8_t force)
+{
+
+}
+
+void grippers_open(grippers_t gripper)
+{
+  if(gripper==LEFT_GRIPPER)
+  {
+    current_angles[L_GRIPPER]=left_open_value<<16;
+    ram_set_bit(DARWIN_GRIPPER_CONTROL,0);
+  }
+  else
+  {
+    current_angles[R_GRIPPER]=right_open_value<<16;
+    ram_set_bit(DARWIN_GRIPPER_CONTROL,2);
+  }
+}
+
+void grippers_close(grippers_t gripper)
+{
+  if(gripper==LEFT_GRIPPER)
+  {
+    current_angles[L_GRIPPER]=left_close_value<<16;
+    ram_set_bit(DARWIN_GRIPPER_CONTROL,1);
+  }
+  else
+  {
+    current_angles[R_GRIPPER]=right_close_value<<16;
+    ram_set_bit(DARWIN_GRIPPER_CONTROL,3);
+  }
+}
+
diff --git a/src/head_tracking.c b/src/head_tracking.c
new file mode 100644
index 0000000000000000000000000000000000000000..8acf565fc1e0bb66b9485d1fc3a7116e0228f105
--- /dev/null
+++ b/src/head_tracking.c
@@ -0,0 +1,147 @@
+#include "head_tracking.h"
+#include "ram.h"
+#include "motion_manager.h"
+
+#define DEFAULT_PAN_P    40//
+#define DEFAULT_PAN_I    0//
+#define DEFAULT_PAN_D    0//
+
+#define DEFAULT_TILT_P   40//
+#define DEFAULT_TILT_I   0//
+#define DEFAULT_TILT_D   0//
+
+// private variables
+int64_t prev_error_pan;// 48|16
+int64_t prev_error_tilt;// 48|16
+int64_t integral_pan;
+int64_t integral_tilt;
+uint8_t pan_p;
+uint8_t pan_i;
+uint8_t pan_d;
+uint8_t tilt_p;
+uint8_t tilt_i;
+uint8_t tilt_d;
+int64_t pan_target;// 48|16
+int64_t tilt_target;// 48|16
+int64_t head_tracking_period;
+uint8_t head_tracking_enabled;
+
+// public functions
+void head_tracking_init(uint16_t period)
+{
+  /* initialize private variables */
+  prev_error_pan=0;
+  prev_error_tilt=0;
+  integral_pan=0;
+  integral_tilt=0;
+  pan_p=DEFAULT_PAN_P;
+  ram_write_byte(DARWIN_HEAD_PAN_P,DEFAULT_PAN_P);
+  pan_i=DEFAULT_PAN_I;
+  ram_write_byte(DARWIN_HEAD_PAN_I,DEFAULT_PAN_I);
+  pan_d=DEFAULT_PAN_D;
+  ram_write_byte(DARWIN_HEAD_PAN_D,DEFAULT_PAN_D);
+  tilt_p=DEFAULT_TILT_P;
+  ram_write_byte(DARWIN_HEAD_TILT_P,DEFAULT_TILT_P);
+  tilt_i=DEFAULT_TILT_I;
+  ram_write_byte(DARWIN_HEAD_TILT_I,DEFAULT_TILT_I);
+  tilt_d=DEFAULT_TILT_D;
+  ram_write_byte(DARWIN_HEAD_TILT_D,DEFAULT_TILT_D);
+  pan_target=0;
+  tilt_target=0;
+  head_tracking_period=period;
+  head_tracking_enabled=0x00;
+}
+
+void head_tracking_start(void)
+{
+  head_tracking_enabled=0x01;
+}
+
+void head_tracking_stop(void)
+{
+  head_tracking_enabled=0x00;
+}
+
+void head_tracking_set_period(int16_t period)
+{
+  head_tracking_period=period;
+}
+
+void head_tracking_set_pan_p(uint8_t value)
+{
+  pan_p=value;
+  ram_write_byte(DARWIN_HEAD_PAN_P,value);
+}
+
+void head_tracking_set_pan_i(uint8_t value)
+{
+  pan_i=value;
+  ram_write_byte(DARWIN_HEAD_PAN_I,value);
+}
+
+void head_tracking_set_pan_d(uint8_t value)
+{
+  pan_d=value;
+  ram_write_byte(DARWIN_HEAD_PAN_D,value);
+}
+
+void head_tracking_set_tilt_p(uint8_t value)
+{
+  tilt_p=value;
+  ram_write_byte(DARWIN_HEAD_TILT_P,value);
+}
+
+void head_tracking_set_tilt_i(uint8_t value)
+{
+  tilt_i=value;
+  ram_write_byte(DARWIN_HEAD_TILT_I,value);
+}
+
+void head_tracking_set_tilt_d(uint8_t value)
+{
+  tilt_d=value;
+  ram_write_byte(DARWIN_HEAD_TILT_D,value);
+}
+
+void head_tracking_set_target_pan(int16_t target)
+{
+  pan_target=target<<9;
+  ram_write_word(DARWIN_HEAD_PAN_L,target);
+}
+
+void head_tracking_set_target_tilt(int16_t target)
+{
+  tilt_target=target<<9;
+  ram_write_word(DARWIN_HEAD_TILT_L,target);
+}
+
+// motion manager process function
+void head_tracking_process(void)
+{
+  int64_t pan_error, tilt_error;
+  int64_t derivative_pan=0, derivative_tilt=0;
+
+  if(head_tracking_enabled)
+  {
+    ram_set_bit(DARWIN_HEAD_STATUS,0x01);
+    if(manager_get_module(HEAD_PAN)==MM_HEAD)
+    {
+      pan_error = pan_target-current_angles[HEAD_PAN];
+      integral_pan += (pan_error*head_tracking_period)>>16;
+      derivative_pan = ((pan_error - prev_error_pan)<<16)/head_tracking_period;
+      current_angles[HEAD_PAN]+=(pan_p*pan_error+pan_i*integral_pan+pan_d*derivative_pan)>>10;
+      prev_error_pan = pan_error;
+    }
+    if(manager_get_module(HEAD_TILT)==MM_HEAD)
+    { 
+      tilt_error = tilt_target-current_angles[HEAD_TILT];
+      integral_tilt += (tilt_error*head_tracking_period)>>16;
+      derivative_tilt = ((tilt_error - prev_error_tilt)<<16)/head_tracking_period;
+      current_angles[HEAD_TILT]+=(tilt_p*tilt_error+tilt_i*integral_tilt+tilt_d*derivative_tilt)>>10;
+      prev_error_tilt = tilt_error;
+    }
+  }
+  else
+    ram_clear_bit(DARWIN_HEAD_STATUS,0x01);
+}
+
diff --git a/src/imu.c b/src/imu.c
index c7c6a3960cc09242ea647a082d2abd463c614187..dfe58628627a36d361893ae6c8ea68c89303a92c 100755
--- a/src/imu.c
+++ b/src/imu.c
@@ -104,9 +104,10 @@ const uint8_t gyro_conf_len=6;
 uint8_t gyro_conf_data[6];
 const uint8_t gyro_data_reg=GYRO_OUT_X_L;
 const uint8_t gyro_data_len=6;
-int16_t gyro_x_center;
-int16_t gyro_y_center;
-int16_t gyro_z_center;
+int16_t gyro_center[3];
+int32_t gyro_data[3];
+uint8_t gyro_calibration;
+const uint16_t gyro_cal_num_samples=1024; 
 // accelerometer variables
 const uint8_t accel_conf_reg=ACCEL_CTRL_REG1;
 const uint8_t accel_conf_len=5;
@@ -247,6 +248,19 @@ void imu_accel_get_data(void)
   imu_spi_start_read(accel_data_reg,accel_data_len);
 }
 
+void imu_accel_convert_data(uint8_t *data_in,uint8_t *data_out)
+{
+  uint8_t i;
+  int16_t value;
+
+  for(i=0;i<3;i++)
+  {
+    value=(data_in[i*2]+(data_in[i*2+1]<<8))<<1;
+    data_out[i*2]=value%256;
+    data_out[i*2+1]=value/256;
+  }
+}
+
 // gyroscope private functions
 uint8_t imu_gyro_detect(void)
 {
@@ -302,6 +316,18 @@ void imu_gyro_get_data(void)
   imu_spi_start_read(gyro_data_reg,gyro_data_len);
 }
 
+void imu_gyro_convert_data(uint8_t *data_in,uint8_t *data_out)
+{
+  uint8_t i;
+
+  for(i=0;i<3;i++)
+  {
+    gyro_data[i]=(((int16_t)(data_in[i*2]+(data_in[i*2+1]<<8)))*70)-gyro_center[i];
+    data_out[i*2]=gyro_data[i]%256;
+    data_out[i*2+1]=gyro_data[i]/256;
+  }
+}
+
 // interrupt handlers
 void IMU_SPI_IRQHANDLER(void)
 {
@@ -335,10 +361,13 @@ void IMU_SPI_IRQHANDLER(void)
 
 void IMU_TIMER_IRQHandler(void)
 {
-  static imu_state_t state=IMU_GET_GYRO;
+  static imu_state_t state=IMU_GET_ACCEL;
+  static int32_t gyro_accum[3];
+  static int16_t num_samples=0;
+  uint8_t data[6];
 
   TIM_ClearITPendingBit(IMU_TIMER,TIM_IT_Update);
- 
+
   if(imu_is_op_done())
   {
     if(imu_stop_flag==0x01)
@@ -355,12 +384,44 @@ void IMU_TIMER_IRQHandler(void)
     {
       switch(state)
       {
-        case IMU_GET_ACCEL: imu_spi_get_data(&ram_data[DARWIN_IMU_GYRO_X_L]);
-                            imu_accel_get_data();
+        case IMU_GET_ACCEL: if(ram_data[DARWIN_IMU_STATUS]&0x02)
+                            {
+                              imu_spi_get_data(data);
+                              imu_gyro_convert_data(data,&ram_data[DARWIN_IMU_GYRO_X_L]);
+                              if(gyro_calibration)
+                              {
+                                gyro_accum[0]+=gyro_data[0];
+                                gyro_accum[1]+=gyro_data[1];
+                                gyro_accum[2]+=gyro_data[2];
+                                num_samples++;
+                                if(num_samples==gyro_cal_num_samples)
+                                {
+                                  num_samples=0;
+                                  gyro_center[0]=gyro_accum[0]/gyro_cal_num_samples;
+                                  gyro_center[1]=gyro_accum[1]/gyro_cal_num_samples;
+                                  gyro_center[2]=gyro_accum[2]/gyro_cal_num_samples;
+                                  gyro_accum[0]=0;
+                                  gyro_accum[1]=0;
+                                  gyro_accum[2]=0;
+                                  ram_set_bit(DARWIN_IMU_STATUS,3);
+                                  ram_clear_bit(DARWIN_IMU_CONTROL,1);
+                                  if((ram_data[DARWIN_IMU_STATUS]&0x04)==0x00)
+                                    imu_stop_flag=0x01;
+                                  gyro_calibration=0x00;
+                                }
+                              }
+                            }
+                            if(ram_data[DARWIN_IMU_STATUS]&0x01)
+                              imu_accel_get_data();
                             state=IMU_GET_GYRO;
                             break;
-        case IMU_GET_GYRO: imu_spi_get_data(&ram_data[DARWIN_IMU_ACCEL_X_L]);
-                           imu_gyro_get_data();
+        case IMU_GET_GYRO: if(ram_data[DARWIN_IMU_STATUS]&0x01)
+                           {
+                             imu_spi_get_data(data);
+                             imu_accel_convert_data(data,&ram_data[DARWIN_IMU_ACCEL_X_L]);
+                           }
+                           if(ram_data[DARWIN_IMU_STATUS]&0x02)
+                             imu_gyro_get_data();
                            state=IMU_GET_ACCEL;
                            break;
       }
@@ -391,11 +452,11 @@ void imu_init(void)
   GPIO_InitStructure.GPIO_Speed = GPIO_Speed_50MHz;
   GPIO_InitStructure.GPIO_Mode = GPIO_Mode_Out_PP;
   GPIO_Init(IMU_ACCEL_CS_PORT, &GPIO_InitStructure);
+  GPIO_SetBits(IMU_ACCEL_CS_PORT,IMU_ACCEL_CS_PIN);
 
   GPIO_InitStructure.GPIO_Pin = IMU_GYRO_CS_PIN;
   GPIO_Init(IMU_GYRO_CS_PORT, &GPIO_InitStructure);
   // disable both sensors
-  GPIO_SetBits(IMU_ACCEL_CS_PORT,IMU_ACCEL_CS_PIN);
   GPIO_SetBits(IMU_GYRO_CS_PORT,IMU_GYRO_CS_PIN);
 
   // initliaze the SPI peripheral
@@ -421,6 +482,10 @@ void imu_init(void)
   }
   imu_op_state=IMU_DONE;
   imu_current_device_id=0x00;
+  gyro_center[0]=0;
+  gyro_center[1]=0;
+  gyro_center[2]=0;
+  gyro_calibration=0x00;
 
   // configure the SPI module
   SPI_StructInit(&SPI_InitStructure);
@@ -430,15 +495,15 @@ void imu_init(void)
   SPI_InitStructure.SPI_CPOL = SPI_CPOL_High;
   SPI_InitStructure.SPI_CPHA = SPI_CPHA_2Edge;
   SPI_InitStructure.SPI_NSS = SPI_NSS_Soft;
-  SPI_InitStructure.SPI_BaudRatePrescaler = SPI_BaudRatePrescaler_8;
+  SPI_InitStructure.SPI_BaudRatePrescaler = SPI_BaudRatePrescaler_256;
   SPI_InitStructure.SPI_FirstBit = SPI_FirstBit_MSB;
   SPI_InitStructure.SPI_CRCPolynomial = 7;
   SPI_Init(IMU_SPI, &SPI_InitStructure);
 
   /* Configure the SPI interrupt priority */
   NVIC_InitStructure.NVIC_IRQChannel = IMU_SPI_IRQn;
-  NVIC_InitStructure.NVIC_IRQChannelPreemptionPriority = 3;
-  NVIC_InitStructure.NVIC_IRQChannelSubPriority = 3;
+  NVIC_InitStructure.NVIC_IRQChannelPreemptionPriority = 0;
+  NVIC_InitStructure.NVIC_IRQChannelSubPriority = 1;
   NVIC_InitStructure.NVIC_IRQChannelCmd = ENABLE;
   NVIC_Init(&NVIC_InitStructure);
   SPI_I2S_ITConfig(IMU_SPI, SPI_I2S_IT_RXNE , ENABLE);
@@ -454,41 +519,75 @@ void imu_init(void)
   TIM_TimeBaseInit(IMU_TIMER,&TIM_TimeBaseStructure);
 
   NVIC_InitStructure.NVIC_IRQChannel = IMU_TIMER_IRQn;
-  NVIC_InitStructure.NVIC_IRQChannelPreemptionPriority = 3;
+  NVIC_InitStructure.NVIC_IRQChannelPreemptionPriority = 0;
   NVIC_InitStructure.NVIC_IRQChannelSubPriority = 0;
   NVIC_InitStructure.NVIC_IRQChannelCmd = ENABLE;
   NVIC_Init(&NVIC_InitStructure);
 
+  if(imu_gyro_detect())
+  {  
+    imu_gyro_get_config();
+    imu_gyro_set_config();
+  }
   if(imu_accel_detect())
   {
     imu_accel_get_config();
     imu_accel_set_config();
   }
   // detect the sensors
-  if(imu_gyro_detect())
-  {
-    imu_gyro_get_config();
-    imu_gyro_set_config();
-  }
 }
 
 void imu_start(void)
 {
-  // start the accelerometer
-  imu_accel_start();
-  imu_gyro_start();
-
-  // start the timer to get the imu data
-  TIM_Cmd(IMU_TIMER, ENABLE);
-  TIM_ITConfig(IMU_TIMER, TIM_IT_Update, ENABLE);
-  ram_set_bit(DARWIN_IMU_STATUS,2);
+  if((ram_data[DARWIN_IMU_STATUS]&0x04)==0x00)// imu is not running
+  {
+    // start the accelerometer
+    imu_accel_start();
+    imu_gyro_start();
+
+    // start the timer to get the imu data
+    TIM_Cmd(IMU_TIMER, ENABLE);
+    TIM_ITConfig(IMU_TIMER, TIM_IT_Update, ENABLE);
+    ram_set_bit(DARWIN_IMU_STATUS,2);
+    ram_set_bit(DARWIN_IMU_CONTROL,0);
+  }
 }
 
 void imu_stop(void)
 {
-  imu_stop_flag=0x01;
-  while(!imu_stopped);
-  imu_stopped=0x00;
-  imu_accel_stop();
-  imu_gyro_stop();
+  if(ram_data[DARWIN_IMU_STATUS]&0x04)
+  {
+    imu_stop_flag=0x01;
+    while(!imu_stopped);
+    imu_stopped=0x00;
+    imu_accel_stop();
+    imu_gyro_stop();
+    ram_clear_bit(DARWIN_IMU_CONTROL,0);
+  }
+}
+
+void imu_start_gyro_cal(void)
+{
+  if((ram_data[DARWIN_IMU_STATUS]&0x04)==0x00)// imu is not running
+  {
+    // start the accelerometer
+    imu_accel_start();
+    imu_gyro_start();
+
+    // start the timer to get the imu data
+    TIM_Cmd(IMU_TIMER, ENABLE);
+    TIM_ITConfig(IMU_TIMER, TIM_IT_Update, ENABLE);
+  }
+  ram_clear_bit(DARWIN_IMU_STATUS,3);
+  gyro_center[0]=0;
+  gyro_center[1]=0;
+  gyro_center[2]=0;
+  gyro_calibration=0x01;
+}
+
+void imu_get_gyro_data(int32_t *x, int32_t *y, int32_t *z)
+{
+  *x=gyro_data[0];
+  *y=gyro_data[1];
+  *z=gyro_data[2];
 }
diff --git a/src/joint_motion.c b/src/joint_motion.c
new file mode 100644
index 0000000000000000000000000000000000000000..0672858f8856f35ff03948b60824ac0433f4c637
--- /dev/null
+++ b/src/joint_motion.c
@@ -0,0 +1,180 @@
+#include "joint_motion.h"
+#include "motion_manager.h"
+#include "ram.h"
+#include "gpio.h"
+#include <stdlib.h>
+
+// private variables
+int64_t joint_target_angles[MANAGER_MAX_NUM_SERVOS];//48|16
+int64_t joint_target_speeds[MANAGER_MAX_NUM_SERVOS];//48|16
+int64_t joint_target_accels[MANAGER_MAX_NUM_SERVOS];//48|16
+
+int64_t joint_current_angles[MANAGER_MAX_NUM_SERVOS];//48|16
+int64_t joint_current_speeds[MANAGER_MAX_NUM_SERVOS];//48|16
+
+int8_t joint_dir[MANAGER_MAX_NUM_SERVOS];// joint_direction
+
+uint8_t joint_stop;
+uint8_t joint_moving;
+int64_t joint_motion_period;
+
+// public functions
+void joint_motion_init(uint16_t period)
+{
+  uint8_t i;
+
+  /* initialize the internal variables */
+  for(i=0;i<MANAGER_MAX_NUM_SERVOS;i++)
+  {
+    // target values
+    joint_target_angles[i]=current_angles[i];
+    joint_target_speeds[i]=0;
+    joint_target_accels[i]=0;
+    // current values
+    joint_current_angles[i]=current_angles[i];
+    joint_current_speeds[i]=0;
+    // the current angles, speeds and accelerations are in RAM
+    joint_dir[i]=0;
+  }
+  joint_stop=0x00;
+  joint_motion_period=period;
+}
+
+void joint_motion_set_period(uint16_t period)
+{
+  joint_motion_period=period;
+}
+
+uint16_t joint_motion_get_period(void)
+{
+  return joint_motion_period;
+}
+
+inline void joint_motion_load_target_angle(uint8_t servo_id, int16_t angle)
+{
+  if(angle>manager_get_ccw_angle_limit(servo_id))
+    angle=manager_get_ccw_angle_limit(servo_id);
+  else if(angle<manager_get_cw_angle_limit(servo_id))
+    angle=manager_get_cw_angle_limit(servo_id);
+  joint_target_angles[servo_id]=angle<<9;
+}
+
+inline void joint_motion_load_target_speed(uint8_t servo_id, uint8_t speed)
+{
+  joint_target_speeds[servo_id]=speed<<16;
+}
+
+inline void joint_motion_load_target_accel(uint8_t servo_id, uint8_t accel)
+{
+  joint_target_accels[servo_id]=accel<<16;
+}
+
+void joint_motion_start(void)
+{
+  uint8_t i;
+
+  /* initialize the internal variables */
+  for(i=0;i<MANAGER_MAX_NUM_SERVOS;i++)
+  {
+    // current values
+    joint_current_angles[i]=current_angles[i];
+    // the current angles, speeds and accelerations are in RAM
+    if(joint_target_angles[i]>=joint_current_angles[i])
+      joint_dir[i]=1;
+    else
+      joint_dir[i]=-1;
+  }
+  joint_moving=0x01;
+  ram_set_bit(DARWIN_JOINTS_CONTROL,0);
+  ram_clear_bit(DARWIN_JOINTS_CONTROL,1);
+  ram_set_bit(DARWIN_JOINTS_STATUS,0);
+}
+
+void joint_motion_stop(void)
+{
+  joint_stop=0x01;
+  ram_set_bit(DARWIN_JOINTS_CONTROL,1);
+}
+
+uint8_t are_joints_moving(void)
+{
+  return joint_moving;
+}
+
+// motion manager interface functions
+void joint_motion_process(void)
+{
+  uint8_t moving=0x00,i;
+
+  if(joint_moving==0x01)
+  {
+    for(i=0;i<MANAGER_MAX_NUM_SERVOS;i++)
+    {
+      if(manager_get_module(i)==MM_JOINTS)
+      {
+        if(joint_stop==0x01)
+        {
+          joint_target_angles[i]=current_angles[i];
+          joint_target_speeds[i]=0;
+          joint_target_accels[i]=0;
+          joint_current_speeds[i]=0;
+        }
+        else
+        {
+          if(abs(joint_target_angles[i]-joint_current_angles[i])>=6553)// 0.5 degrees
+          {  
+            moving=0x01;
+            if(joint_current_speeds[i]!=joint_dir[i]*joint_target_speeds[i])
+            {
+              if(joint_current_speeds[i]>joint_dir[i]*joint_target_speeds[i])
+              {
+                joint_current_speeds[i]=joint_current_speeds[i]-((joint_target_accels[i]*joint_motion_period)>>16);
+                if(joint_current_speeds[i]<joint_dir[i]*joint_target_speeds[i])
+                  joint_current_speeds[i]=joint_dir[i]*joint_target_speeds[i];
+              }
+              else
+              {
+                joint_current_speeds[i]=joint_current_speeds[i]+((joint_target_accels[i]*joint_motion_period)>>16);
+                if(joint_current_speeds[i]>joint_dir[i]*joint_target_speeds[i])
+                  joint_current_speeds[i]=joint_dir[i]*joint_target_speeds[i];
+              }
+              joint_current_angles[i]=joint_current_angles[i]+((joint_current_speeds[i]*joint_motion_period)>>16);
+              if(joint_dir[i]==1 && joint_current_angles[i]>joint_target_angles[i])
+                joint_current_angles[i]=joint_target_angles[i];
+              if(joint_dir[i]==-1 && joint_current_angles[i]<joint_target_angles[i])
+                joint_current_angles[i]=joint_target_angles[i];
+            }
+            else
+            {
+              if(abs(joint_target_angles[i]-joint_current_angles[i])>(joint_target_speeds[i]*joint_target_speeds[i]/(2*joint_target_accels[i])))
+              {
+                joint_current_angles[i]=joint_current_angles[i]+((joint_current_speeds[i]*joint_motion_period)>>16);
+                if(joint_dir[i]==1 && joint_current_angles[i]>joint_target_angles[i])
+                  joint_current_angles[i]=joint_target_angles[i];
+                if(joint_dir[i]==-1 && joint_current_angles[i]<joint_target_angles[i])
+                  joint_current_angles[i]=joint_target_angles[i];
+              }
+              else
+              {
+                joint_target_speeds[i]=joint_target_speeds[i]-((joint_target_accels[i]*joint_motion_period)>>16);
+                joint_current_speeds[i]=joint_dir[i]*joint_target_speeds[i];
+                joint_current_angles[i]=joint_current_angles[i]+((joint_current_speeds[i]*joint_motion_period)>>16);
+                if(joint_dir[i]==1 && joint_current_angles[i]>joint_target_angles[i])
+                  joint_current_angles[i]=joint_target_angles[i];
+                if(joint_dir[i]==-1 && joint_current_angles[i]<joint_target_angles[i])
+                  joint_current_angles[i]=joint_target_angles[i];
+              }
+            }
+          }
+          current_angles[i]=joint_current_angles[i];
+        }
+      }
+    }
+    if(moving==0)
+    {
+      joint_moving=0x00;
+      ram_clear_bit(DARWIN_JOINTS_STATUS,0);
+    }
+  }
+}
+
diff --git a/src/motion_manager.c b/src/motion_manager.c
index de103bc1c928266a40ecbcac89d6f1a872a0b6ea..17ceba7f5aa979d78c63da1e3d5f975cc7a074f9 100755
--- a/src/motion_manager.c
+++ b/src/motion_manager.c
@@ -1,10 +1,15 @@
 #include "motion_manager.h"
 #include "dynamixel_master_uart_dma.h"
+#include "dynamixel_slave_uart_dma.h"
 #include "dyn_servos.h"
 #include "ram.h"
 #include "action.h"
 #include "walking.h"
+#include "joint_motion.h"
+#include "head_tracking.h"
 #include "time.h"
+#include "imu.h"
+#include "gpio.h"
 
 #define      MOTION_TIMER                 TIM5
 #define      MOTION_TIMER_IRQn            TIM5_IRQn
@@ -15,6 +20,16 @@
 __IO uint16_t manager_motion_period;
 uint8_t manager_num_servos;
 TServoInfo manager_servos[MANAGER_MAX_NUM_SERVOS];
+int64_t current_angles[MANAGER_MAX_NUM_SERVOS];
+int64_t balance_offset[MANAGER_MAX_NUM_SERVOS];
+TBulkData bulk_data[MANAGER_MAX_NUM_SERVOS];
+uint8_t balance_enabled;
+
+// balane variables
+float knee_gain=0.3;
+float ankle_pitch_gain=0.9;
+float hip_roll_gain=0.5;
+float ankle_roll_gain=1.0;
 
 // private functions
 void manager_send_motion_command(void)
@@ -23,9 +38,22 @@ void manager_send_motion_command(void)
   uint8_t data[256];
   uint8_t i,num=0;
   
-  for(i=0;i<manager_num_servos;i++)
+  for(i=0;i<MANAGER_MAX_NUM_SERVOS;i++)
+  {
+    if(manager_servos[i].enabled && manager_servos[i].model!=0x0000 && manager_servos[i].dyn_version==0x02)
+    {
+      servo_ids[num]=manager_servos[i].id;
+      data[num*2]=manager_servos[i].current_value%256;
+      data[num*2+1]=manager_servos[i].current_value/256;
+      num++;
+    }
+  }
+  if(num>0)
+    dyn2_master_sync_write(num,servo_ids,XL_GOAL_POSITION_L,2,data);
+  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 && manager_servos[i].dyn_version==0x01)
     {
       servo_ids[num]=manager_servos[i].id;
       data[num*2]=manager_servos[i].current_value%256;
@@ -61,17 +89,37 @@ inline uint16_t manager_value_to_speed(uint8_t servo_id,uint16_t value)
 
 void manager_get_current_position(void)
 {
-  uint8_t i;
+  uint8_t i,num=0,id;
 
-  for(i=0;i<manager_num_servos;i++)
+  for(i=0;i<27;i++)
   {
-    if(!manager_servos[i].enabled)// servo is disabled
+    if((!manager_servos[i].enabled && manager_servos[i].model!=0x0000 && manager_servos[i].dyn_version==0x01) || manager_servos[i].module==MM_HEAD)// servo is disabled
     {
-      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);
+      bulk_data[num].id=manager_servos[i].id;
+      bulk_data[num].length=2;
+      bulk_data[num].address=P_PRESENT_POSITION_L;
+      bulk_data[num].data=(uint8_t *)&manager_servos[i].current_value;
+      num++;
+    }
+    else
+    {
+      ram_data[DARWIN_MM_SERVO0_CUR_POS_L+i*2]=(manager_servos[i].current_angle&0xFF);
+      ram_data[DARWIN_MM_SERVO0_CUR_POS_H+i*2]=(manager_servos[i].current_angle>>8);
+    }
+  }
+  if(num>0)
+  {
+    if(dyn_master_bulk_read(num,bulk_data)==DYN_SUCCESS)
+    {
+      for(i=0;i<num;i++)
+      {
+        id=bulk_data[i].id;
+        manager_servos[id].current_angle=manager_value_to_angle(id,manager_servos[id].current_value);
+        current_angles[id]=manager_servos[id].current_angle<<9;
+        ram_data[DARWIN_MM_SERVO0_CUR_POS_L+id*2]=(manager_servos[id].current_angle&0xFF);
+        ram_data[DARWIN_MM_SERVO0_CUR_POS_H+id*2]=(manager_servos[id].current_angle>>8);
+      }
     }
-    ram_data[DARWIN_MM_SERVO0_CUR_POS_L+i*2]=(manager_servos[i].current_angle&0xFF);
-    ram_data[DARWIN_MM_SERVO0_CUR_POS_H+i*2]=(manager_servos[i].current_angle>>8);
   }
 }
 
@@ -79,49 +127,77 @@ void manager_get_target_position(void)
 {
   uint8_t i;
 
-  for(i=0;i<manager_num_servos;i++)
+  for(i=0;i<MANAGER_MAX_NUM_SERVOS;i++)
   {
     if(manager_servos[i].enabled)// servo is enabled
     {
-      if(manager_servos[i].module==MM_ACTION)
-      {
-        manager_servos[i].current_angle=((action_current_angles[i+1])>>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);
-      }
-      else if(manager_servos[i].module==MM_WALKING)
-      {
-        manager_servos[i].current_angle=walking_current_angles[i+1];
-        manager_servos[i].current_value=manager_angle_to_value(i,manager_servos[i].current_angle);
-      }
+      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);
     }
   }
 }
 
+void manager_balance(void)
+{
+  int32_t gyro_x,gyro_y,gyro_z;
+ 
+  if(balance_enabled)
+  {
+    imu_get_gyro_data(&gyro_x,&gyro_y,&gyro_z);
+    // filter out some noise
+    if(gyro_x>0 && gyro_x<4096)
+      gyro_x=0;
+    else if(gyro_x<0 && gyro_x>-4096)
+      gyro_x=0;
+    if(gyro_y>0 && gyro_y<4096)
+      gyro_y=0;
+    else if(gyro_y<0 && gyro_y>-4096)
+      gyro_y=0;
+
+    balance_offset[R_HIP_ROLL]=(int32_t)(gyro_x*hip_roll_gain*6.428571); // R_HIP_ROLL
+    balance_offset[L_HIP_ROLL]=(int32_t)(gyro_x*hip_roll_gain*6.428571); // L_HIP_ROLL
+    balance_offset[R_KNEE]=(int32_t)(gyro_y*knee_gain*-6.428571); // R_KNEE
+    balance_offset[L_KNEE]=(int32_t)(gyro_y*knee_gain*6.428571); // L_KNEE
+
+    balance_offset[R_ANKLE_PITCH]=(int32_t)(gyro_y*ankle_pitch_gain*6.428571); // R_ANKLE_PITCH
+    balance_offset[L_ANKLE_PITCH]=(int32_t)(gyro_y*ankle_pitch_gain*-6.428571); // L_ANKLE_PITCH        
+    balance_offset[R_ANKLE_ROLL]=(int32_t)(gyro_x*ankle_roll_gain*6.428571); // R_ANKLE_ROLL
+    balance_offset[L_ANKLE_ROLL]=(int32_t)(gyro_x*ankle_roll_gain*6.428571); // L_ANKLE_ROLL
+  }
+}
+
 // interrupt handlers
 void MOTION_TIMER_IRQHandler(void)
 {
-  uint16_t capture;
+  uint16_t capture_start,capture_end;
+  int16_t elapsed;
 
   if(TIM_GetITStatus(MOTION_TIMER, TIM_IT_CC1)!=RESET)
   {
     TIM_ClearITPendingBit(MOTION_TIMER,TIM_IT_CC1);
-    capture = TIM_GetCapture1(MOTION_TIMER);
-    TIM_SetCompare1(MOTION_TIMER, capture + manager_motion_period);
+    capture_start = TIM_GetCapture1(MOTION_TIMER);
+    TIM_SetCompare1(MOTION_TIMER, capture_start + manager_motion_period);
+    // get the disabled servos position
+    manager_get_current_position();
     // call the action process
     action_process();
     // call the joint motion process
-    // joint_motion_process();
+    joint_motion_process();
     // call the walking process
     walking_process();
+    // head tracking process
+    head_tracking_process();
     // balance the robot
-    // manager_balance();
+    manager_balance();
     // get the target angles from all modules
     manager_get_target_position();
     // send the motion commands to the servos
     manager_send_motion_command();
-    // get the disabled servos position
-//    manager_get_current_position();
+    capture_end = TIM_GetCounter(MOTION_TIMER);
+    elapsed=capture_end-capture_start;
+    if(elapsed<0) elapsed+=0xFFFFF;
+    ram_write_word(DARWIN_MM_CUR_PERIOD,(uint16_t)elapsed);
   }
 }
 
@@ -132,8 +208,8 @@ 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);
 
@@ -144,12 +220,13 @@ void manager_init(uint16_t period_us)
   // detect the servos connected 
   dyn_master_scan(&num,servo_ids); 
   ram_data[DARWIN_MM_NUM_SERVOS]=num;
+  manager_num_servos=0;
   for(i=0;i<MANAGER_MAX_NUM_SERVOS;i++)
   {
-    if(i<num)
+    if(i==servo_ids[manager_num_servos])
     {
       // read the model of the i-th device
-      dyn_master_read_word(servo_ids[i],P_MODEL_NUMBER_L,&model);
+      dyn_master_read_word(servo_ids[manager_num_servos],P_MODEL_NUMBER_L,&model);
       switch(model)
       {
         case SERVO_AX12A: manager_servos[i].encoder_resolution=1023;
@@ -214,22 +291,28 @@ void manager_init(uint16_t period_us)
                           break;
         default: break; 
       }
-      manager_servos[i].id=servo_ids[i];
+      manager_servos[i].id=servo_ids[manager_num_servos];
       manager_servos[i].model=model;
       manager_servos[i].module=MM_NONE;
       manager_servos[i].enabled=0x00;
+      manager_servos[i].dyn_version=0x01;
       // get the servo's current position
       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);
       ram_data[DARWIN_MM_SERVO0_CUR_POS_L+i*2]=(manager_servos[i].current_angle&0xFF);
       ram_data[DARWIN_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+1]=manager_servos[i].current_angle<<9;
+      current_angles[i]=manager_servos[i].current_angle<<9;
       manager_num_servos++;
     }
     else
     {
-      manager_servos[i].id=servo_ids[i];
+      manager_servos[i].id=i;
       manager_servos[i].model=0x0000;
       manager_servos[i].module=MM_NONE;
       manager_servos[i].encoder_resolution=0;
@@ -240,35 +323,94 @@ 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;
+      manager_servos[i].dyn_version=0x00;
+    }
+  }
+  // scan for version 2 servos
+  dyn2_master_scan(&num,servo_ids);
+  ram_data[DARWIN_MM_NUM_SERVOS]=manager_num_servos;
+  manager_num_servos=0;
+  for(i=0;i<MANAGER_MAX_NUM_SERVOS;i++)
+  {
+    if(i==servo_ids[manager_num_servos])
+    {
+      // read the model of the i-th device
+      dyn2_master_read_word(servo_ids[manager_num_servos],XL_MODEL_NUMBER_L,&model);
+      switch(model)
+      {
+        case SERVO_XL320: manager_servos[i].encoder_resolution=1023;
+                          manager_servos[i].gear_ratio=238;
+                          manager_servos[i].max_angle=300<<7;
+                          manager_servos[i].center_angle=150<<7;
+                          manager_servos[i].max_speed=690;
+                          break;
+        default: break;
+      }
+      manager_servos[i].id=servo_ids[manager_num_servos];
+      manager_servos[i].model=model;
+      manager_servos[i].module=MM_NONE;
+      manager_servos[i].enabled=0x00;
+      manager_servos[i].dyn_version=0x02;
+      // get the servo's current position
+      dyn2_master_read_word(manager_servos[i].id,XL_PRESENT_POSITION_L,&manager_servos[i].current_value);
+      manager_servos[i].current_angle=manager_value_to_angle(i,manager_servos[i].current_value);
+      ram_data[DARWIN_MM_SERVO0_CUR_POS_L+i*2]=(manager_servos[i].current_angle&0xFF);
+      ram_data[DARWIN_MM_SERVO0_CUR_POS_H+i*2]=(manager_servos[i].current_angle>>8);
+      // read the servo limits
+      dyn2_master_read_word(manager_servos[i].id,XL_CW_ANGLE_LIMIT_L,&value);
+      manager_servos[i].cw_angle_limit=manager_value_to_angle(i,value);
+      dyn2_master_read_word(manager_servos[i].id,XL_CCW_ANGLE_LIMIT_L,&value);
+      manager_servos[i].ccw_angle_limit=manager_value_to_angle(i,value);
+      // set the action current angles
+      current_angles[i]=manager_servos[i].current_angle<<9;
+      manager_num_servos++;
     }
   }
   dyn_master_disable_power();
-  manager_num_servos=num; 
+  manager_num_servos=num+ram_data[DARWIN_MM_NUM_SERVOS];
+  ram_data[DARWIN_MM_NUM_SERVOS]=((num&0x07)<<5)+(ram_data[DARWIN_MM_NUM_SERVOS]&0x1F);
 
   // initialize the timer interrupts
   NVIC_InitStructure.NVIC_IRQChannel = MOTION_TIMER_IRQn;
   NVIC_InitStructure.NVIC_IRQChannelPreemptionPriority = 3;
-  NVIC_InitStructure.NVIC_IRQChannelSubPriority = 0;
+  NVIC_InitStructure.NVIC_IRQChannelSubPriority = 3;
   NVIC_InitStructure.NVIC_IRQChannelCmd = ENABLE;
   NVIC_Init(&NVIC_InitStructure);
 
   /* motion timer configuration */
   TIM_TimeBaseStructure.TIM_Period = 0xFFFF;
-  TIM_TimeBaseStructure.TIM_Prescaler = 36;
+  TIM_TimeBaseStructure.TIM_Prescaler = 72;
   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(DARWIN_MM_STATUS,0);
+
+  /* initialize motion modules */
+  action_init(period_us);
+  walking_init(manager_motion_period);
+  joint_motion_init(period_us);
+  head_tracking_init(period_us);
+
+  ram_write_byte(DARWIN_MM_KNEE_GAIN,(uint8_t)(knee_gain*64.0));
+  ram_write_byte(DARWIN_MM_ANKLE_PITCH_GAIN,(uint8_t)(ankle_pitch_gain*64.0));
+  ram_write_byte(DARWIN_MM_HIP_ROLL_GAIN,(uint8_t)(hip_roll_gain*64.0));
+  ram_write_byte(DARWIN_MM_ANKLE_ROLL_GAIN,(uint8_t)(ankle_roll_gain*64.0));
+
+  for(i=0;i<MANAGER_MAX_NUM_SERVOS;i++)
+    balance_offset[i]=0;
+  balance_enabled=0x01;
+  ram_data[DARWIN_MM_CONTROL]=0x02;
 }
 
 uint16_t manager_get_period(void)
@@ -278,14 +420,24 @@ 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(DARWIN_MM_PERIOD_H,period_us);
+  action_set_period(period_us);
+  walking_set_period(manager_motion_period);
+  joint_motion_set_period(period_us);
+  head_tracking_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(DARWIN_MM_STATUS,0);
+  ram_set_bit(DARWIN_MM_CONTROL,0);
 }
 
 inline void manager_disable(void)
@@ -293,6 +445,7 @@ inline void manager_disable(void)
   TIM_Cmd(MOTION_TIMER, DISABLE);
   TIM_ITConfig(MOTION_TIMER, TIM_IT_CC1, DISABLE);
   ram_clear_bit(DARWIN_MM_STATUS,0);
+  ram_clear_bit(DARWIN_MM_CONTROL,0);
 }
 
 inline uint8_t manager_is_enabled(void)
@@ -300,6 +453,20 @@ inline uint8_t manager_is_enabled(void)
   return ram_data[DARWIN_MM_STATUS]&0x01;
 }
 
+void manager_enable_balance(void)
+{
+  ram_set_bit(DARWIN_MM_STATUS,1);
+  ram_set_bit(DARWIN_MM_CONTROL,1);
+  balance_enabled=0x01;
+}
+
+void manager_disable_balance(void)
+{
+  ram_clear_bit(DARWIN_MM_STATUS,1);
+  ram_clear_bit(DARWIN_MM_CONTROL,1);
+  balance_enabled=0x00;
+}
+
 inline uint8_t manager_get_num_servos(void)
 {
   return manager_num_servos;
@@ -307,8 +474,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(DARWIN_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(DARWIN_MM_MODULE_EN0+servo_id/2,byte);
+  }
 }
 
 inline TModules manager_get_module(uint8_t servo_id)
@@ -321,14 +498,34 @@ 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(DARWIN_MM_MODULE_EN0+servo_id/2,&byte);
+    if(servo_id%2)
+      byte|=0x08;
+    else
+      byte|=0x80;
+    ram_write_byte(DARWIN_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(DARWIN_MM_MODULE_EN0+servo_id/2,&byte);
+    if(servo_id%2)
+      byte&=0xF7;
+    else
+      byte&=0x7F;
+    ram_write_byte(DARWIN_MM_MODULE_EN0+servo_id/2,byte);
+  }
 }
 
 void manager_disable_servos(void)
@@ -339,7 +536,7 @@ void manager_disable_servos(void)
 
   for(i=0;i<manager_num_servos;i++)
   {
-    if(!manager_servos[i].enabled)
+    if(!manager_servos[i].enabled && manager_servos[i].dyn_version==0x01)
     {
       servo_ids[num]=manager_servos[i].id;
       data[num]=0x00;
@@ -358,3 +555,42 @@ 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;
+}
+
+void manager_set_knee_gain(uint8_t gain)
+{
+  ram_data[DARWIN_MM_KNEE_GAIN]=gain;
+  knee_gain=gain/64.0;
+}
+
+void manager_set_ankle_pitch_gain(uint8_t gain)
+{
+  ram_data[DARWIN_MM_ANKLE_PITCH_GAIN]=gain;
+  ankle_pitch_gain=gain/64.0;
+}
+
+void manager_set_hip_roll_gain(uint8_t gain)
+{
+  ram_data[DARWIN_MM_HIP_ROLL_GAIN]=gain;
+  hip_roll_gain=gain/64.0;
+}
+
+void manager_set_ankle_roll_gain(uint8_t gain)
+{
+  ram_data[DARWIN_MM_ANKLE_ROLL_GAIN]=gain;
+  ankle_roll_gain=gain/64.0;
+}
+
+uint8_t manager_get_dyn_version(uint8_t servo_id)
+{
+  return manager_servos[servo_id].dyn_version;
+}
+
diff --git a/src/ram.c b/src/ram.c
index 0bdd49976e589d126604cae8c42e207f25854b58..65cf3bc6a3ffc215f9179f2d3f102e573a79d72e 100755
--- a/src/ram.c
+++ b/src/ram.c
@@ -4,8 +4,10 @@ uint8_t ram_data[RAM_SIZE];
 
 void ram_init(void)
 {
-  uint16_t eeprom_data;
+  uint16_t eeprom_data,i;
 
+  for(i=0;i<RAM_SIZE;i++)
+    ram_data[i]=0x00;
   // read contents from EEPROM to RAM
   if(EE_ReadVariable(DEVICE_MODEL_OFFSET,&eeprom_data)==0)
     ram_data[DEVICE_MODEL_OFFSET]=(uint8_t)(eeprom_data&0x00FF);
diff --git a/src/walking.c b/src/walking.c
index 2c3a264235dafb8fba2a6ea14042479a37838248..13d03c12bb971f658d568c638f3fe2de9ccd2fac 100755
--- a/src/walking.c
+++ b/src/walking.c
@@ -3,11 +3,10 @@
 #include "ram.h"
 #include "darwin_kinematics.h"
 #include "darwin_math.h"
+#include "time.h"
 
 enum {PHASE0 = 0,PHASE1 = 1,PHASE2 = 2, PHASE3 = 3};
 
-int64_t walking_current_angles[MANAGER_MAX_NUM_SERVOS];
-
 // leg kinematics
 const uint8_t servo_seq[14]={R_HIP_YAW,R_HIP_ROLL,R_HIP_PITCH,R_KNEE,R_ANKLE_PITCH,R_ANKLE_ROLL,L_HIP_YAW,L_HIP_ROLL,L_HIP_PITCH,L_KNEE,L_ANKLE_PITCH,L_ANKLE_ROLL,R_SHOULDER_PITCH,L_SHOULDER_PITCH};
 
@@ -18,7 +17,7 @@ float Y_SWAP_AMPLITUDE=20.0;//mm
 float Z_SWAP_AMPLITUDE=5;//mm
 float ARM_SWING_GAIN=1.5;
 float PELVIS_OFFSET=3.0;//degrees
-float HIP_PITCH_OFFSET=23.0;//degrees
+float HIP_PITCH_OFFSET=0;//23.0;//degrees
 
 uint8_t P_GAIN=32;
 uint8_t I_GAIN=0;
@@ -33,15 +32,21 @@ float P_OFFSET=0;
 float R_OFFSET=0;
 
 // Walking control
-float PERIOD_TIME=600;
+float PERIOD_TIME=600;//milliseconds
 float DSP_RATIO=0.1;
 float STEP_FB_RATIO=0.3;
-float X_MOVE_AMPLITUDE=10;//mm
+float X_MOVE_AMPLITUDE=0;//mm
+float current_x_amplitude=0;
 float Y_MOVE_AMPLITUDE=0;//mm
+float current_y_amplitude=0;
 float Z_MOVE_AMPLITUDE=40;//mm
 float A_MOVE_AMPLITUDE=0;//degrees
+float current_a_amplitude=0;
 uint8_t A_MOVE_AIM_ON=0x00;
 
+float max_acceleration=0.02;// m/s/s
+float max_rot_accel=0.02;// rad/s/s
+
 // private variables
 float m_PeriodTime;
 float m_DSP_Ratio;
@@ -98,6 +103,7 @@ float m_Arm_Swing_Gain;
 
 volatile uint8_t m_Ctrl_Running;
 float m_Time;
+float walking_period;
 
 uint8_t m_Phase;
 float m_Body_Swing_Y;
@@ -106,7 +112,7 @@ float m_Body_Swing_Z;
 // private functions
 inline float wsin(float time, float period, float period_shift, float mag, float mag_shift)
 {
-  return mag * sin(2 * 3.141592 / period * time - period_shift) + mag_shift;
+  return mag * sin(((2.0*PI*time)/period)-period_shift) + mag_shift;
 }
 
 void update_param_time()
@@ -115,15 +121,16 @@ void update_param_time()
   m_DSP_Ratio = DSP_RATIO;
   m_SSP_Ratio = 1 - DSP_RATIO;
 
+  m_SSP_Time = m_PeriodTime * m_SSP_Ratio;
+
   m_X_Swap_PeriodTime = m_PeriodTime / 2.0;
-  m_X_Move_PeriodTime = m_PeriodTime * m_SSP_Ratio;
+  m_X_Move_PeriodTime = m_SSP_Time;
   m_Y_Swap_PeriodTime = m_PeriodTime;
-  m_Y_Move_PeriodTime = m_PeriodTime * m_SSP_Ratio;
+  m_Y_Move_PeriodTime = m_SSP_Time;
   m_Z_Swap_PeriodTime = m_PeriodTime / 2.0;
-  m_Z_Move_PeriodTime = m_PeriodTime * m_SSP_Ratio / 2.0;
-  m_A_Move_PeriodTime = m_PeriodTime * m_SSP_Ratio;
+  m_Z_Move_PeriodTime = m_SSP_Time / 2.0;
+  m_A_Move_PeriodTime = m_SSP_Time;
 
-  m_SSP_Time = m_PeriodTime * m_SSP_Ratio;
   m_SSP_Time_Start_L = (1 - m_SSP_Ratio) * m_PeriodTime / 4.0;
   m_SSP_Time_End_L = (1 + m_SSP_Ratio) * m_PeriodTime / 4.0;
   m_SSP_Time_Start_R = (3 - m_SSP_Ratio) * m_PeriodTime / 4.0;
@@ -141,12 +148,36 @@ void update_param_time()
 
 void update_param_move()
 {
-  // Forward/Back
-  m_X_Move_Amplitude = X_MOVE_AMPLITUDE;
-  m_X_Swap_Amplitude = X_MOVE_AMPLITUDE * STEP_FB_RATIO;
+  // change longitudinal and transversal velocities to get to the target ones
+  if(current_x_amplitude<X_MOVE_AMPLITUDE)
+  {
+    current_x_amplitude+=max_acceleration*PERIOD_TIME;
+    if(current_x_amplitude>X_MOVE_AMPLITUDE)
+      current_x_amplitude=X_MOVE_AMPLITUDE;
+  }
+  else if(current_x_amplitude>X_MOVE_AMPLITUDE)
+  {
+    current_x_amplitude-=max_acceleration*PERIOD_TIME;
+    if(current_x_amplitude<X_MOVE_AMPLITUDE)
+      current_x_amplitude=X_MOVE_AMPLITUDE;
+  }
+  m_X_Move_Amplitude = current_x_amplitude;
+  m_X_Swap_Amplitude = current_x_amplitude * STEP_FB_RATIO;
 
   // Right/Left
-  m_Y_Move_Amplitude = Y_MOVE_AMPLITUDE / 2.0;
+  if(current_y_amplitude<Y_MOVE_AMPLITUDE)
+  {
+    current_y_amplitude+=max_acceleration*PERIOD_TIME;
+    if(current_y_amplitude>Y_MOVE_AMPLITUDE)
+      current_y_amplitude=Y_MOVE_AMPLITUDE;
+  }
+  else if(current_y_amplitude>Y_MOVE_AMPLITUDE)
+  {
+    current_y_amplitude-=max_acceleration*PERIOD_TIME;
+    if(current_y_amplitude<Y_MOVE_AMPLITUDE)
+      current_y_amplitude=Y_MOVE_AMPLITUDE;
+  }
+  m_Y_Move_Amplitude = current_y_amplitude / 2.0;
   if(m_Y_Move_Amplitude > 0)
     m_Y_Move_Amplitude_Shift = m_Y_Move_Amplitude;
   else
@@ -159,6 +190,19 @@ void update_param_move()
   m_Z_Swap_Amplitude_Shift = m_Z_Swap_Amplitude;
 
   // Direction
+  if(current_a_amplitude<A_MOVE_AMPLITUDE)
+  {
+    current_a_amplitude+=max_acceleration*PERIOD_TIME;
+    if(current_a_amplitude>A_MOVE_AMPLITUDE)
+      current_a_amplitude=A_MOVE_AMPLITUDE;
+  }
+  else if(current_a_amplitude>A_MOVE_AMPLITUDE)
+  {
+    current_a_amplitude-=max_acceleration*PERIOD_TIME;
+    if(current_a_amplitude<A_MOVE_AMPLITUDE)
+      current_a_amplitude=A_MOVE_AMPLITUDE;
+  }
+
   if(A_MOVE_AIM_ON == 0x00)
   {
     m_A_Move_Amplitude = A_MOVE_AMPLITUDE * PI / 180.0 / 2.0;
@@ -189,30 +233,32 @@ void update_param_balance()
 }
 
 // public functions
-void walking_init(void)
+void walking_init(uint16_t period)
 {
   // initialize RAM
-  ram_write_byte(DARWIN_X_OFFSET,(uint8_t)X_OFFSET);
-  ram_write_byte(DARWIN_Y_OFFSET,(uint8_t)Y_OFFSET);
-  ram_write_byte(DARWIN_Z_OFFSET,(uint8_t)Z_OFFSET);
-  ram_write_byte(DARWIN_ROLL,(uint8_t)(A_OFFSET*10));
-  ram_write_byte(DARWIN_PITCH,(uint8_t)(P_OFFSET*10));
-  ram_write_byte(DARWIN_YAW,(uint8_t)(R_OFFSET*10));
-  ram_write_word(DARWIN_HIP_PITCH_OFF_L,(uint16_t)(HIP_PITCH_OFFSET*10));
-  ram_write_word(DARWIN_PERIOD_TIME_L,(uint16_t)PERIOD_TIME);
-  ram_write_byte(DARWIN_DSP_RATIO,(uint8_t)(DSP_RATIO*10));
-  ram_write_byte(DARWIN_STEP_FW_BW_RATIO,(uint8_t)(STEP_FB_RATIO*100));
-  ram_write_byte(DARWIN_STEP_FW_BW,(uint8_t)(X_MOVE_AMPLITUDE));
-  ram_write_byte(DARWIN_STEP_LEFT_RIGHT,(uint8_t)(Y_MOVE_AMPLITUDE));
-  ram_write_byte(DARWIN_STEP_DIRECTION,(uint8_t)(A_MOVE_AMPLITUDE));
-  ram_write_byte(DARWIN_FOOT_HEIGHT,(uint8_t)(Z_MOVE_AMPLITUDE));
-  ram_write_byte(DARWIN_SWING_RIGHT_LEFT,(uint8_t)(Y_SWAP_AMPLITUDE*10));
-  ram_write_byte(DARWIN_SWING_TOP_DOWN,(uint8_t)(Z_SWAP_AMPLITUDE*10));
-  ram_write_byte(DARWIN_PELVIS_OFFSET,(uint8_t)(PELVIS_OFFSET));
-  ram_write_byte(DARWIN_ARM_SWING_GAIN,(uint8_t)(ARM_SWING_GAIN*10));
-  ram_write_byte(DARWIN_P_GAIN,(uint8_t)(P_GAIN));
-  ram_write_byte(DARWIN_I_GAIN,(uint8_t)(I_GAIN));
-  ram_write_byte(DARWIN_D_GAIN,(uint8_t)(D_GAIN));
+  ram_write_byte(DARWIN_X_OFFSET,(uint8_t)X_OFFSET);//mm
+  ram_write_byte(DARWIN_Y_OFFSET,(uint8_t)Y_OFFSET);//mm
+  ram_write_byte(DARWIN_Z_OFFSET,(uint8_t)Z_OFFSET);//mm
+  ram_write_byte(DARWIN_ROLL,(uint8_t)(A_OFFSET*8));//degrees format 5|3
+  ram_write_byte(DARWIN_PITCH,(uint8_t)(P_OFFSET*8));//degrees format 5|3
+  ram_write_byte(DARWIN_YAW,(uint8_t)(R_OFFSET*8));//degrees  format 5|3
+  ram_write_word(DARWIN_HIP_PITCH_OFF_L,(uint16_t)(HIP_PITCH_OFFSET*1024));//degrees format 6|10
+  ram_write_word(DARWIN_PERIOD_TIME_L,(uint16_t)PERIOD_TIME);// milliseconds
+  ram_write_byte(DARWIN_DSP_RATIO,(uint8_t)(DSP_RATIO*128));//ratio [0,1] format 1|7
+  ram_write_byte(DARWIN_STEP_FW_BW_RATIO,(uint8_t)(STEP_FB_RATIO*128));//ratio [0,1] format 1|7
+  ram_write_byte(DARWIN_STEP_FW_BW,(uint8_t)(X_MOVE_AMPLITUDE));//mm
+  ram_write_byte(DARWIN_STEP_LEFT_RIGHT,(uint8_t)(Y_MOVE_AMPLITUDE));//mm
+  ram_write_byte(DARWIN_STEP_DIRECTION,(uint8_t)(A_MOVE_AMPLITUDE*8));//degrees 5|3
+  ram_write_byte(DARWIN_FOOT_HEIGHT,(uint8_t)(Z_MOVE_AMPLITUDE));//mm
+  ram_write_byte(DARWIN_SWING_RIGHT_LEFT,(uint8_t)(Y_SWAP_AMPLITUDE));//mm
+  ram_write_byte(DARWIN_SWING_TOP_DOWN,(uint8_t)(Z_SWAP_AMPLITUDE));//mm
+  ram_write_byte(DARWIN_PELVIS_OFFSET,(uint8_t)(PELVIS_OFFSET*8));//degrees 5|3
+  ram_write_byte(DARWIN_ARM_SWING_GAIN,(uint8_t)(ARM_SWING_GAIN*32));// gain 3|5
+  ram_write_byte(DARWIN_P_GAIN,(uint8_t)(P_GAIN));// servo gain
+  ram_write_byte(DARWIN_I_GAIN,(uint8_t)(I_GAIN));// servo gain
+  ram_write_byte(DARWIN_D_GAIN,(uint8_t)(D_GAIN));// servo gain
+  ram_write_byte(DARWIN_MAX_ACC,(uint8_t)(max_acceleration*1024));// maximum linear acceleration
+  ram_write_byte(DARWIN_MAX_ROT_ACC,(uint8_t)(max_rot_accel*1024));// maximum rotational acceleration
   // initialize the internal variables
   m_X_Swap_Phase_Shift=PI;
   m_X_Swap_Amplitude_Shift=0.0;
@@ -227,6 +273,7 @@ void walking_init(void)
   m_A_Move_Phase_Shift=PI/2.0;
 
   m_Time=0;
+  walking_period=((double)period)/1000.0;
 
   m_Body_Swing_Y=0.0;
   m_Body_Swing_Z=0.0;
@@ -239,53 +286,98 @@ void walking_init(void)
   walking_process();
 }
 
-void walking_set_param(uint8_t param_id,uint16_t param_value)
-{
-  
-}
-
-void walking_enable_autobalance(void)
-{
-
-}
-
-void walking_disable_autobalance(void)
+inline void walking_set_period(uint16_t period)
 {
-
+  walking_period=((double)period)/1000.0;
 }
 
-uint8_t walking_is_autobalance_enabled(void)
+inline uint16_t walking_get_period(void)
 {
-  return 0x00;
+  return (uint16_t)(walking_period*1000);
 }
 
-void walking_enable_aim(void)
+void walking_set_param(uint8_t param_id,int8_t param_value)
 {
+  uint16_t value;
 
-}
-
-void walking_disable_aim(void)
-{
-
-}
-
-uint8_t walking_is_aim_enabled(void)
-{
-  return 0x00;
+  switch(param_id)
+  {
+    case DARWIN_X_OFFSET: X_OFFSET=param_value;
+                          break;
+    case DARWIN_Y_OFFSET: Y_OFFSET=param_value;
+                          break;
+    case DARWIN_Z_OFFSET: Z_OFFSET=param_value;
+                          break;
+    case DARWIN_ROLL: A_OFFSET=((float)param_value)/8.0;
+                      break;
+    case DARWIN_PITCH: P_OFFSET=((float)param_value)/8.0;
+                       break;
+    case DARWIN_YAW: R_OFFSET=((float)param_value)/8.0;
+                     break;
+    case DARWIN_HIP_PITCH_OFF_L: value=(uint16_t)(HIP_PITCH_OFFSET*1024);
+                                 value&=0xFF00;
+                                 value+=param_value;
+                                 HIP_PITCH_OFFSET=((float)value)/1024.0;
+                                 break;
+    case DARWIN_HIP_PITCH_OFF_H: value=(uint16_t)(HIP_PITCH_OFFSET*1024);
+                                 value&=0x00FF;
+                                 value+=param_value*256;
+                                 HIP_PITCH_OFFSET=((float)value)/1024.0;
+                                 break;
+    case DARWIN_PERIOD_TIME_L: value=(uint16_t)PERIOD_TIME;
+                               value&=0xFF00;
+                               value+=param_value;
+                               PERIOD_TIME=value;
+                               break;
+    case DARWIN_PERIOD_TIME_H: value=(uint16_t)PERIOD_TIME;
+                               value&=0x00FF;
+                               value+=param_value*256;
+                               PERIOD_TIME=value;
+                               break;
+    case DARWIN_DSP_RATIO: DSP_RATIO=((float)param_value)/128.0;
+                           break;
+    case DARWIN_STEP_FW_BW_RATIO: STEP_FB_RATIO=((float)param_value)/128.0;
+                                  break;
+    case DARWIN_STEP_FW_BW: X_MOVE_AMPLITUDE=param_value;
+                            break;
+    case DARWIN_STEP_LEFT_RIGHT: Y_MOVE_AMPLITUDE=param_value;
+                                 break;
+    case DARWIN_STEP_DIRECTION: A_MOVE_AMPLITUDE=((float)param_value)/8.0;
+                                break;
+    case DARWIN_FOOT_HEIGHT: Z_MOVE_AMPLITUDE=param_value;
+                             break;
+    case DARWIN_SWING_RIGHT_LEFT: Y_SWAP_AMPLITUDE=param_value;
+                                  break;
+    case DARWIN_SWING_TOP_DOWN: Z_SWAP_AMPLITUDE=param_value;
+                                break;
+    case DARWIN_PELVIS_OFFSET: PELVIS_OFFSET=((float)param_value)/8.0;
+                               break;
+    case DARWIN_ARM_SWING_GAIN: ARM_SWING_GAIN=((float)param_value)/32.0;
+                                break;
+    case DARWIN_P_GAIN: P_GAIN=param_value;
+                        break;
+    case DARWIN_I_GAIN: I_GAIN=param_value;
+                        break;
+    case DARWIN_D_GAIN: D_GAIN=param_value;
+                        break;
+    case DARWIN_MAX_ACC: max_acceleration=param_value/1024.0;
+                         break;
+    case DARWIN_MAX_ROT_ACC: max_rot_accel=param_value/1024.0;
+                             break;
+  }  
+  ram_write_byte(param_id,param_value);
 }
 
 void walking_start(void)
 {
-  ram_write_byte(DARWIN_WALK_STATUS,0x01);
-  ram_set_bit(DARWIN_WALK_CONTROL,0x00);
-  ram_clear_bit(DARWIN_WALK_CONTROL,0x01);
+  ram_set_bit(DARWIN_WALK_STATUS,0);
+  ram_set_bit(DARWIN_WALK_CONTROL,0);
   m_Ctrl_Running=0x01;
 }
 
 void walking_stop(void)
 {
-  ram_set_bit(DARWIN_WALK_CONTROL,0x01);
-  ram_clear_bit(DARWIN_WALK_CONTROL,0x00);
+  ram_set_bit(DARWIN_WALK_CONTROL,1);
   m_Ctrl_Running=0x00;
 }
 
@@ -327,12 +419,12 @@ void walking_process(void)
       }
     }
   }
-  else if(m_Time >= (m_Phase_Time1 - manager_get_period()/2.0) && m_Time < (m_Phase_Time1 + manager_get_period()/2.0))
+  else if(m_Time >= (m_Phase_Time1 - walking_period/2.0) && m_Time < (m_Phase_Time1 + walking_period/2.0))
   {
     update_param_move();
     m_Phase = PHASE1;
   }
-  else if(m_Time >= (m_Phase_Time2 - manager_get_period()/2.0) && m_Time < (m_Phase_Time2 + manager_get_period()/2.0))
+  else if(m_Time >= (m_Phase_Time2 - walking_period/2.0) && m_Time < (m_Phase_Time2 + walking_period/2.0))
   {
     update_param_time();
     m_Time = m_Phase_Time2;
@@ -349,7 +441,7 @@ void walking_process(void)
       }
     }
   }
-  else if(m_Time >= (m_Phase_Time3 - manager_get_period()/2.0) && m_Time < (m_Phase_Time3 + manager_get_period()/2.0))
+  else if(m_Time >= (m_Phase_Time3 - walking_period/2.0) && m_Time < (m_Phase_Time3 + walking_period/2.0))
   {
     update_param_move();
     m_Phase = PHASE3;
@@ -472,7 +564,7 @@ void walking_process(void)
   ram_read_byte(DARWIN_WALK_STATUS,&walking);
   if(walking == 0x01)
   {
-    m_Time += 7.8;//manager_get_period();
+    m_Time += walking_period;
     if(m_Time >= m_PeriodTime)
       m_Time = 0;
   }
@@ -489,14 +581,17 @@ void walking_process(void)
   // Compute motor value
   for(i=0; i<14; i++)
   {
-    if(i==1)
-      walking_current_angles[servo_seq[i]]=(initAngle[i]+(float)dir[i]*(angle[i] + pelvis_offset_r))*128;// format 9|7
-    else if(i==7)
-      walking_current_angles[servo_seq[i]]=(initAngle[i]+(float)dir[i]*(angle[i] + pelvis_offset_l))*128;// format 9|7
-    else if(i==2 || i==8)
-      walking_current_angles[servo_seq[i]]=(initAngle[i]+(float)dir[i]*(angle[i] - HIP_PITCH_OFFSET))*128;// format 9|7
-    else
-      walking_current_angles[servo_seq[i]]=(initAngle[i]+(float)dir[i] * angle[i])*128;//format 9|7
+    if(manager_get_module(i)==MM_WALKING)
+    {
+      if(i==1)
+        current_angles[servo_seq[i]]=(initAngle[i]+(float)dir[i]*(angle[i] + pelvis_offset_r))*65536;// format 9|7
+      else if(i==7)
+        current_angles[servo_seq[i]]=(initAngle[i]+(float)dir[i]*(angle[i] + pelvis_offset_l))*65536;// format 9|7
+      else if(i==2 || i==8)
+        current_angles[servo_seq[i]]=(initAngle[i]+(float)dir[i]*(angle[i] - HIP_PITCH_OFFSET))*65536;// format 9|7
+      else
+        current_angles[servo_seq[i]]=(initAngle[i]+(float)dir[i] * angle[i])*65536;//format 9|7
+    }
   }
 }