diff --git a/include/action.h b/include/action.h
deleted file mode 100755
index 6a3817af3a1509331ce20e4953bbf37f25c20e96..0000000000000000000000000000000000000000
--- a/include/action.h
+++ /dev/null
@@ -1,34 +0,0 @@
-#ifndef _ACTION_H
-#define _ACTION_H
-
-#ifdef __cplusplus
-extern "C" {
-#endif
-
-#include "stm32f1xx.h"
-#include "motion_pages.h"
-
-// public functions
-void action_init(uint16_t period_us);
-void action_set_period(uint16_t period_us);
-uint16_t action_get_period(void);
-uint8_t action_load_page(uint8_t page_id);
-void action_start_page(void);
-void action_stop_page(void);
-uint8_t action_is_running(void);
-uint8_t action_get_current_page(void);
-uint8_t action_get_current_step(void);
-void action_enable_int(void);
-void action_disable_int(void);
-uint8_t action_is_int_enabled(void);
-// operation functions
-uint8_t action_in_range(unsigned short int address, unsigned short int length);
-void action_process_write_cmd(unsigned short int address,unsigned short int length,unsigned char *data);
-// motion manager interface
-void action_process(void);
-
-#ifdef __cplusplus
-}
-#endif
-
-#endif
diff --git a/include/darwin_kinematics.h b/include/darwin_kinematics.h
deleted file mode 100755
index 783781e8a49a95ab0aecd55691071913b5096959..0000000000000000000000000000000000000000
--- a/include/darwin_kinematics.h
+++ /dev/null
@@ -1,24 +0,0 @@
-#ifndef _DARWIN_KINEMATICS_H
-#define _DARWIN_KINEMATICS_H
-
-#ifdef __cplusplus
-extern "C" {
-#endif
-
-#include "stm32f1xx.h"
-
-#define DARWIN_PELVIS_LENGTH               29.0 //mm
-#define DARWIN_LEG_SIDE_OFFSET             37.0 //mm
-#define DARWIN_THIGH_LENGTH                93.0 //mm
-#define DARWIN_CALF_LENGTH                 93.0 //mm
-#define DARWIN_ANKLE_LENGTH                33.5 //mm
-#define DARWIN_LEG_LENGTH                  (DARWIN_THIGH_LENGTH + DARWIN_CALF_LENGTH + DARWIN_ANKLE_LENGTH) //mm (THIGH_LENGTH + CALF_LENGTH + ANKLE_LENGTH)
-
-// public functions
-uint8_t darwin_leg_ik(float *out, float x, float y, float z, float a, float b, float c);
-
-#ifdef __cplusplus
-}
-#endif
-
-#endif
diff --git a/include/darwin_math.h b/include/darwin_math.h
deleted file mode 100755
index 82ef927c3d8c6a225cfe6c18086c37e10ea5d4a6..0000000000000000000000000000000000000000
--- a/include/darwin_math.h
+++ /dev/null
@@ -1,70 +0,0 @@
-#ifndef _DARWIN_MATH_H
-#define _DARWIN_MATH_H
-
-#ifdef __cplusplus
-extern "C" {
-#endif
-
-#include "stm32f1xx.h"
-
-#define   PI    3.14159
-
-typedef struct{
-  float x;
-  float y;
-  float z;
-}TPoint3D;
-
-// point public functions
-void point3d_init(TPoint3D *point);
-void point3d_load(TPoint3D *point, float x, float y, float z);
-void point3d_copy(TPoint3D *point_dst, TPoint3D *point_src);
-
-typedef struct {
-  float x;
-  float y;
-  float z;
-}TVector3D;
-
-// vector public functions
-void vector3d_init(TVector3D *vector);
-void vector3d_load(TVector3D *vector, float x, float y, float z);
-void vector3d_copy(TVector3D *vector_dst, TVector3D *vector_src);
-float vector3d_length(TVector3D *vector);
-
-enum{
-  m00=0,
-  m01,
-  m02,
-  m03,
-  m10,
-  m11,
-  m12,
-  m13,
-  m20,
-  m21,
-  m22,
-  m23,
-  m30,
-  m31,
-  m32,
-  m33,
-};
-
-typedef struct{
-  float coef[16];
-}TMatrix3D;
-
-// matrix public functions
-void matrix3d_init(TMatrix3D *matrix);
-void matrix3d_copy(TMatrix3D *matrix_dst, TMatrix3D *matrix_src);
-void matrix3d_identity(TMatrix3D *matrix);
-uint8_t matrix3d_inverse(TMatrix3D *org, TMatrix3D *inv);
-void matrix3d_set_transform(TMatrix3D *matrix, TPoint3D *point, TVector3D *vector);
-void matrix3d_mult(TMatrix3D *dst,TMatrix3D *a,TMatrix3D *b);
-
-#ifdef __cplusplus
-}
-#endif
-
-#endif
diff --git a/include/grippers.h b/include/grippers.h
deleted file mode 100644
index b8818bd90bffa713d8cdb8b80bdaaa3769a4eedd..0000000000000000000000000000000000000000
--- a/include/grippers.h
+++ /dev/null
@@ -1,34 +0,0 @@
-#ifndef _GRIPPERS_H
-#define _GRIPPERS_H
-
-#ifdef __cplusplus
-extern "C" {
-#endif
-
-#include "stm32f1xx.h"
-#include "motion_manager.h"
-
-typedef enum {GRIPPER_LEFT,GRIPPER_RIGHT} grippers_t;
-
-// public functions
-void grippers_init(uint16_t period_us);
-void grippers_set_period(uint16_t period_us);
-uint16_t grippers_get_period(void);
-void grippers_open(grippers_t gripper);
-void grippers_close(grippers_t gripper);
-void grippers_stop(grippers_t gripper);
-uint8_t gripper_is_open(grippers_t gripper);
-uint8_t gripper_is_close(grippers_t gripper);
-uint8_t gripper_is_moving(grippers_t gripper);
-
-// operation functions
-uint8_t grippers_in_range(unsigned short int address, unsigned short int length);
-void grippers_process_read_cmd(unsigned short int address,unsigned short int length,unsigned char *data);
-void grippers_process_write_cmd(unsigned short int address,unsigned short int length,unsigned char *data);
-void grippers_process(void);
-
-#ifdef __cplusplus
-}
-#endif
-
-#endif
diff --git a/include/head_tracking.h b/include/head_tracking.h
deleted file mode 100644
index f5562649aa1bc791cf19d7e402df00d8e5f18c62..0000000000000000000000000000000000000000
--- a/include/head_tracking.h
+++ /dev/null
@@ -1,41 +0,0 @@
-#ifndef _HEAD_TRACKING_H
-#define _HEAD_TRACKINH_H
-
-#ifdef __cplusplus
-extern "C" {
-#endif
-
-#include "stm32f1xx.h"
-#include "motion_manager.h"
-
-// public functions
-void head_tracking_init(uint16_t period_us);
-void head_tracking_set_period(uint16_t period_us);
-uint16_t head_tracking_get_period(void);
-void head_tracking_start(void);
-void head_tracking_stop(void);
-uint8_t head_is_tracking(void);
-void head_tracking_set_pan_range(int16_t max_angle,int16_t min_angle);
-void head_tracking_get_pan_range(int16_t *max_angle,int16_t *min_angle);
-void head_tracking_set_pan_pid(uint16_t kp,uint16_t ki,uint16_t kd, uint16_t i_clamp);
-void head_tracking_get_pan_pid(uint16_t *kp,uint16_t *ki,uint16_t *kd, uint16_t *i_clamp);
-void head_tracking_set_tilt_range(int16_t max_angle,int16_t min_angle);
-void head_tracking_get_tilt_range(int16_t *max_angle,int16_t *min_angle);
-void head_tracking_set_tilt_pid(uint16_t kp,uint16_t ki,uint16_t kd, uint16_t i_clamp);
-void head_tracking_get_tilt_pid(uint16_t *kp,uint16_t *ki,uint16_t *kd, uint16_t *i_clamp);
-void head_tracking_set_target_pan(int16_t target);
-void head_tracking_set_target_tilt(int16_t target);
-
-// operation functions
-uint8_t head_tracking_in_range(unsigned short int address, unsigned short int length);
-void head_tracking_process_read_cmd(unsigned short int address,unsigned short int length,unsigned char *data);
-void head_tracking_process_write_cmd(unsigned short int address,unsigned short int length,unsigned char *data);
-
-// motion manager process function
-void head_tracking_process(void);
-
-#ifdef __cplusplus
-}
-#endif
-
-#endif
diff --git a/include/joint_motion.h b/include/joint_motion.h
deleted file mode 100644
index f2f0e1621409487cc494c876ab7d90b55a343028..0000000000000000000000000000000000000000
--- a/include/joint_motion.h
+++ /dev/null
@@ -1,34 +0,0 @@
-#ifndef _JOINT_MOTION_H
-#define _JOINT_MOTION_H
-
-#ifdef __cplusplus
-extern "C" {
-#endif
-
-#include "stm32f1xx.h"
-#include "motion_manager.h"
-
-#define NUM_JOINT_GROUPS 4
-
-// public functions
-void joint_motion_init(uint16_t period_us);
-void joint_motion_set_period(uint16_t period_us);
-uint16_t joint_motion_get_period(void);
-void joint_motion_set_group_servos(uint8_t group_id,uint32_t servos);
-uint32_t joint_motion_get_group_servos(uint8_t group_id);
-void joint_motion_start(uint8_t group_id);
-void joint_motion_stop(uint8_t group_id);
-uint8_t are_joints_moving(uint8_t group_id);
-
-// operation functions
-uint8_t joint_motion_in_range(unsigned short int address, unsigned short int length);
-void joint_motion_process_read_cmd(unsigned short int address,unsigned short int length,unsigned char *data);
-void joint_motion_process_write_cmd(unsigned short int address,unsigned short int length,unsigned char *data);
-// motion manager interface functions
-void joint_motion_process(void);
-
-#ifdef __cplusplus
-}
-#endif
-
-#endif
diff --git a/include/stairs.h b/include/stairs.h
deleted file mode 100755
index a34b4dc37a37d8474c7183b785637bfe81d82d04..0000000000000000000000000000000000000000
--- a/include/stairs.h
+++ /dev/null
@@ -1,31 +0,0 @@
-#ifndef _STAIRS_H
-#define _STAIRS_H
-
-#ifdef __cplusplus
-extern "C" {
-#endif
-
-#include "stm32f1xx.h"
-#include "motion_manager.h"
-
-// public functions
-void stairs_init(uint16_t period_us);
-void stairs_set_period(uint16_t period_us);
-uint16_t stairs_get_period(void);
-void stairs_start(uint8_t up);
-void stairs_stop(void);
-uint8_t is_climbing_stairs(void);
-uint8_t stairs_get_phase(void);
-
-// operation functions
-uint8_t stairs_in_range(unsigned short int address, unsigned short int length);
-void stairs_process_read_cmd(unsigned short int address,unsigned short int length,unsigned char *data);
-void stairs_process_write_cmd(unsigned short int address,unsigned short int length,unsigned char *data);
-// motion manager interface functions
-void stairs_process(void);
-
-#ifdef __cplusplus
-}
-#endif
-
-#endif
diff --git a/include/walking.h b/include/walking.h
deleted file mode 100755
index 30bc04e06e2d59fcf8dc4be369adcad289250b66..0000000000000000000000000000000000000000
--- a/include/walking.h
+++ /dev/null
@@ -1,30 +0,0 @@
-#ifndef _WALKING_H
-#define _WALKING_H
-
-#ifdef __cplusplus
-extern "C" {
-#endif
-
-#include "stm32f1xx.h"
-#include "motion_manager.h"
-
-// public functions
-void walking_init(uint16_t period_us);
-void walking_set_period(uint16_t period_us);
-uint16_t walking_get_period(void);
-void walking_start(void);
-void walking_stop(void);
-uint8_t is_walking(void);
-
-// operation functions
-uint8_t walking_in_range(unsigned short int address, unsigned short int length);
-void walking_process_read_cmd(unsigned short int address,unsigned short int length,unsigned char *data);
-void walking_process_write_cmd(unsigned short int address,unsigned short int length,unsigned char *data);
-// motion manager interface functions
-void walking_process(void);
-
-#ifdef __cplusplus
-}
-#endif
-
-#endif
diff --git a/src/action.c b/src/action.c
deleted file mode 100755
index bfe36ed583094b592b765389de7c77598a6e456a..0000000000000000000000000000000000000000
--- a/src/action.c
+++ /dev/null
@@ -1,602 +0,0 @@
-#include "action.h"
-#include "motion_pages.h"
-#include "motion_manager.h"
-#include "ram.h"
-
-#define SPEED_BASE_SCHEDULE 0x00
-#define TIME_BASE_SCHEDULE  0x0A
-
-// private variables
-typedef enum {ACTION_PRE,ACTION_MAIN,ACTION_POST,ACTION_PAUSE} action_states;
-
-TPage action_next_page;
-TPage action_current_page;
-uint8_t action_current_step_index;
-uint8_t action_current_page_index;
-// angle variables
-int64_t action_moving_angles[PAGE_MAX_NUM_SERVOS];// fixed point 48|16 format
-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
-int64_t action_main_speed[PAGE_MAX_NUM_SERVOS];// fixed point 48|16 format
-// control variables
-uint8_t action_end,action_stop;
-uint8_t action_zero_speed_finish[PAGE_MAX_NUM_SERVOS];
-uint8_t action_next_index;
-uint8_t action_running;
-// time variables (in time units (7.8 ms each time unit))
-int64_t action_total_time;// fixed point 48|16 format
-int64_t action_pre_time;// fixed point 48|16 format
-int64_t action_main_time;// fixed point 48|16 format
-int64_t action_step_time;// fixed point 48|16 format
-int64_t action_pause_time;// fixed point 48|16 format
-int64_t action_current_time;// fixed point 48|16 format
-int64_t action_section_time;// fixed point 48|16 format
-int64_t action_period;
-int16_t action_period_us;
-
-// private functions
-void action_load_next_step(void)
-{
-  // page and step information
-  static int8_t num_repetitions=0;
-  // angle variables
-  int16_t next_angle;// information from the flash memory (fixed point 9|7 format)
-  int64_t max_angle;// fixed point format 48|16 format
-  int64_t tmp_angle;// fixed point format 48|16 format
-  int64_t angle;// fixed point format 48|16 format
-  int64_t target_angles;// fixed point 48|16 format
-  int64_t next_angles;// fixed point 48|16 format
-  int64_t action_pre_time_initial_speed_angle;// fixed point 48|16 format
-  int64_t moving_angle;// fixed point 48|16 format
-  // time variables
-  int64_t accel_time_num;// fixed point 48|16 format
-  int64_t zero_speed_divider;// fixed point 48|16 format
-  int64_t non_zero_speed_divider;// fixed point 48|16 format
-  // control variables
-  uint8_t dir_change;
-  // control information
-  uint8_t i=0;
-
-  action_current_step_index++;
-  if(action_current_step_index>=action_current_page.header.stepnum)// the last step has been executed
-  {
-    if(action_current_page.header.stepnum==1)
-    {
-      if(action_stop)
-        action_next_index=action_current_page.header.exit;
-      else
-      {
-        num_repetitions--;
-        if(num_repetitions>0)
-          action_next_index=action_current_page_index;
-        else
-          action_next_index=action_current_page.header.next;
-      }
-      if(action_next_index==0)
-        action_end=0x01;
-      else
-      {
-        if(action_next_index!=action_current_page_index)
-        {
-          pages_get_page(action_next_index,&action_next_page);
-          ram_data[DARWIN_ACTION_PAGE]=action_next_index;
-          if(!pages_check_checksum(&action_next_page))
-            action_end=0x01;
-        }
-        if(action_next_page.header.repeat==0 || action_next_page.header.stepnum==0)
-          action_end=0x01;
-      }
-    }
-    pages_copy_page(&action_next_page,&action_current_page);// make the next page active
-    if(action_current_page_index!=action_next_index)
-      num_repetitions=action_current_page.header.repeat;
-    action_current_step_index=0;
-    action_current_page_index=action_next_index;
-  }
-  else if(action_current_step_index==action_current_page.header.stepnum-1)// it is the last step
-  {
-    if(action_stop)
-      action_next_index=action_current_page.header.exit;
-    else
-    {
-      num_repetitions--;
-      if(num_repetitions>0)
-        action_next_index=action_current_page_index;
-      else
-        action_next_index=action_current_page.header.next;
-    }
-    if(action_next_index==0)
-      action_end=0x01;
-    else
-    {
-      if(action_next_index!=action_current_page_index)
-      {
-        pages_get_page(action_next_index,&action_next_page);
-        ram_data[DARWIN_ACTION_PAGE]=action_next_index;
-        if(!pages_check_checksum(&action_next_page))
-          action_end=0x01;
-      }
-      if(action_next_page.header.repeat==0 || action_next_page.header.stepnum==0)
-        action_end=0x01;
-    }
-  }
-  ram_data[DARWIN_ACTION_CNTRL]&=0x1F;
-  ram_data[DARWIN_ACTION_CNTRL]|=(action_current_step_index<<5);
-  // compute trajectory
-  action_pause_time=(action_current_page.steps[action_current_step_index].pause*action_current_page.header.speed)>>5;
-  action_pause_time=action_pause_time<<9;
-  action_step_time=(action_current_page.steps[action_current_step_index].time*action_current_page.header.speed)>>5;
-  action_step_time=action_step_time<<9;
-  if(action_step_time<action_period)
-    action_step_time=action_period;// 0.078 in 16|16 format
-  max_angle=0;
-  for(i=0;i<PAGE_MAX_NUM_SERVOS;i++)
-  {
-    if(manager_get_module(i)==MM_ACTION)
-    {
-      angle=action_current_page.steps[action_current_step_index].position[i];
-      if(angle==0x5A00)// bigger than 180
-        target_angles=manager_current_angles[i];
-      else
-        target_angles=angle<<9;
-      action_moving_angles[i]=target_angles-manager_current_angles[i];
-      if(action_end)
-        next_angles=target_angles;
-      else
-      {
-        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(((manager_current_angles[i] < target_angles) && (target_angles < next_angles)) ||
-         ((manager_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];
-        else
-          tmp_angle=action_moving_angles[i];
-        if(tmp_angle>max_angle)
-          max_angle=tmp_angle;
-      }
-      manager_current_slopes[i]=action_current_page.header.slope[i];
-    }
-  }
-  if(action_current_page.header.schedule==SPEED_BASE_SCHEDULE)
-    action_step_time=(max_angle*256)/(action_current_page.steps[action_current_step_index].time*720);
-  accel_time_num=action_current_page.header.accel;
-  accel_time_num=accel_time_num<<9;
-  if(action_step_time<=(accel_time_num<<1))
-  {
-    if(action_step_time==0)
-      accel_time_num=0;
-    else
-    {
-      accel_time_num=(action_step_time-action_period)>>1;
-      if(accel_time_num==0)
-        action_step_time=0;
-    }
-  }
-  action_total_time=action_step_time;
-  action_pre_time=accel_time_num;
-  action_main_time=action_total_time-action_pre_time;
-  non_zero_speed_divider=(action_pre_time>>1)+action_main_time;
-  if(non_zero_speed_divider<action_period)
-    non_zero_speed_divider=action_period;
-  zero_speed_divider=action_main_time;
-  if(zero_speed_divider<action_period)
-    zero_speed_divider=action_period;
-  for(i=0;i<PAGE_MAX_NUM_SERVOS;i++)
-  {
-    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);
-    }
-  }
-}
-
-void action_finish(void)
-{
-  // set the internal state machine to the idle state
-  action_end=0x00;
-  // clear the status falg for the action module
-  ram_data[DARWIN_ACTION_CNTRL]&=(~ACTION_STATUS);
-  // set the interrupt falg for the action module
-  ram_data[DARWIN_ACTION_CNTRL]|=ACTION_INT_FLAG;
-  // change the internal state 
-  action_running=0x00;
-}
-
-// public functions
-void action_init(uint16_t period_us)
-{
-  unsigned char i;
-
-  // init manager_current_angles with the current position of the servos
-  for(i=0;i<PAGE_MAX_NUM_SERVOS;i++)
-  {
-    // angle variables
-    action_moving_angles[i]=0;// fixed point 48|16 format
-    action_start_angles[i]=manager_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
-    // control variables
-    action_zero_speed_finish[i]=0x00;
-  }
-  // clear the contents of the next page
-  pages_clear_page(&action_next_page);
-  pages_clear_page(&action_current_page);
-  action_current_page_index=0;
-  action_current_step_index=-1;
-  // control variables
-  action_end=0x00;
-  action_stop=0x00;
-  action_running=0x00;
-  action_next_index=0;
-  // time variables (in time units (7.8 ms each time unit))
-  action_total_time=0;// fixed point 48|16 format
-  action_pre_time=0;// fixed point 48|16 format
-  action_main_time=0;// fixed point 48|16 format
-  action_step_time=0;// fixed point 48|16 format
-  action_pause_time=0;// fixed point 48|16 format
-  action_current_time=0;// fixed point 48|16 format
-  action_section_time=0;// fixed point 48|16 format
-
-  action_period=(period_us<<16)/1000000;
-  action_period_us=period_us;
-}
-
-void action_set_period(uint16_t period_us)
-{
-  action_period=(period_us<<16)/1000000;
-  action_period_us=period_us;
-}
-
-uint16_t action_get_period(void)
-{
-  return action_period_us;
-}
-
-uint8_t action_load_page(uint8_t page_id)
-{
-  action_next_index=page_id;
-  if(action_next_index<0 || action_next_index>255)
-    return 0x00;
-  pages_get_page(action_next_index,&action_current_page);
-  pages_get_page(action_current_page.header.next,&action_next_page);
-  if(!pages_check_checksum(&action_current_page))
-    return 0x00;
-  if(!pages_check_checksum(&action_next_page))
-    return 0x00;
-  ram_data[DARWIN_ACTION_PAGE]=page_id;
-
-  return 0x01;
-}
-
-void action_start_page(void)
-{
-  uint8_t i;
-
-  for(i=0;i<PAGE_MAX_NUM_SERVOS;i++)
-    action_start_angles[i]=manager_current_angles[i];
-  action_stop=0x00;
-  action_current_time=0;
-  action_section_time=0; 
-  action_current_step_index=-1;
-  ram_data[DARWIN_ACTION_CNTRL]|=ACTION_STATUS;
-  /* clear the interrupt flag */
-  ram_data[DARWIN_ACTION_CNTRL]&=(~ACTION_INT_FLAG);
-  action_running=0x01;
-}
-
-void action_stop_page(void)
-{
-  action_stop=0x01;
-}
-
-uint8_t action_is_running(void)
-{
-  return action_running;
-}
-
-uint8_t action_get_current_page(void)
-{
-  return action_current_page_index;
-}
-
-uint8_t action_get_current_step(void)
-{
-  return action_current_step_index;
-}
-
-void action_enable_int(void)
-{
-  ram_data[DARWIN_ACTION_CNTRL]|=ACTION_INT_EN;
-}
-
-void action_disable_int(void)
-{
-  ram_data[DARWIN_ACTION_CNTRL]&=(~ACTION_INT_EN);
-}
-
-uint8_t action_is_int_enabled(void)
-{
-  if(ram_data[DARWIN_ACTION_CNTRL]&ACTION_INT_EN)
-    return 0x01;
-  else
-    return 0x00;
-}
-
-// motion manager interface
-void action_process(void)
-{
-  uint8_t i;
-  // angle variables 
-  static int64_t accel_angles[PAGE_MAX_NUM_SERVOS];// fixed point 48|16 format
-  static int64_t main_angles[PAGE_MAX_NUM_SERVOS];// fixed point 48|16 format
-  // speed variables (in controller units)
-  static int64_t current_speed[PAGE_MAX_NUM_SERVOS]={0};// fixed point 48|16 format
-  int64_t delta_speed;
-  // control variables 
-  static action_states state=ACTION_PAUSE;
-
-  if(action_running==0x01)
-  {
-    action_current_time+=action_period;
-    switch(state)
-    {
-      case ACTION_PRE: if(action_current_time>action_section_time)
-                       {
-                         for(i=0;i<PAGE_MAX_NUM_SERVOS;i++)
-                         {
-                           if(manager_get_module(i)==MM_ACTION)
-                           {
-                             /* 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;
-                             manager_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]=manager_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]=manager_current_angles[i];
-                             }
-                             /* the first step of the main section */
-                             manager_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);
-                         state=ACTION_MAIN;
-                       }
-                       else
-                       {
-                         for(i=0;i<PAGE_MAX_NUM_SERVOS;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;
-                             manager_current_angles[i]=action_start_angles[i]+accel_angles[i];
-                           }
-                         }
-                         state=ACTION_PRE;
-                       }
-                       break;
-      case ACTION_MAIN: if(action_current_time>action_section_time)
-                        {
-                          for(i=0;i<PAGE_MAX_NUM_SERVOS;i++)
-                          {
-                            if(manager_get_module(i)==MM_ACTION)
-                            {
-                              /* last segment of the main section */
-                              manager_current_angles[i]=action_start_angles[i]+main_angles[i];
-                              current_speed[i]=action_main_speed[i];
-                              /* update state */
-                              action_start_angles[i]=manager_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;
-                                manager_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
-                              {
-                                manager_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;
-                          action_section_time=action_pre_time;
-                          state=ACTION_POST;
-                        }
-                        else
-                        {
-                          for(i=0;i<PAGE_MAX_NUM_SERVOS;i++)
-                          {
-                            if(manager_get_module(i)==MM_ACTION)
-                            {
-                              manager_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;
-                        }
-                        break;
-      case ACTION_POST: if(action_current_time>action_section_time)
-                        {
-                          for(i=0;i<PAGE_MAX_NUM_SERVOS;i++)
-                          {
-                            if(manager_get_module(i)==MM_ACTION)
-                            {
-                              /* 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;
-                                manager_current_angles[i]=action_start_angles[i]+(((action_main_speed[i]*action_section_time)+((delta_speed*action_section_time)>>1))>>16);
-                              }
-                              else
-                              {
-                                manager_current_angles[i]=action_start_angles[i]+main_angles[i];
-                                current_speed[i]=action_main_speed[i];
-                              }
-                              /* update state */
-                              action_start_angles[i]=manager_current_angles[i];
-                              action_start_speed[i]=current_speed[i];
-                            }
-                          }
-                          /* load the next step */
-                          if(action_pause_time==0)
-                          {
-                            if(action_end)
-                            {
-                              state=ACTION_PAUSE;
-                              action_finish();
-                            }
-                            else
-                            {
-                              action_load_next_step();
-                              /* first step of the next section */
-                              for(i=0;i<PAGE_MAX_NUM_SERVOS;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);
-                                  manager_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;
-                              state=ACTION_PRE;
-                            }
-                          }
-                          else
-                          {
-                            action_current_time=action_current_time-action_section_time;
-                            action_section_time=action_pause_time;
-                            state=ACTION_PAUSE;
-                          }
-                        }
-                        else
-                        {
-                          for(i=0;i<PAGE_MAX_NUM_SERVOS;i++)
-                          {
-                            if(manager_get_module(i)==MM_ACTION)
-                            {
-                              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;
-                                manager_current_angles[i]=action_start_angles[i]+(((action_main_speed[i]*action_current_time)+((delta_speed*action_current_time)>>1))>>16);
-                              }
-                              else
-                              {
-                                manager_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;
-                        }
-                        break;
-      case ACTION_PAUSE: if(action_current_time>action_section_time)
-                         {
-                           if(action_end)
-                           {
-                             state=ACTION_PAUSE;
-                             action_finish();
-                           }
-                           else
-                           {
-                             // find next page to execute
-                             action_load_next_step();
-                             /* 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);
-                               manager_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;
-                             state=ACTION_PRE;
-                           }
-                         }
-                         else// pause section still active
-                         {
-                           state=ACTION_PAUSE;
-                         }
-                         break;
-      default: break;
-    }
-  }
-}
-
-// operation functions
-uint8_t action_in_range(unsigned short int address, unsigned short int length)
-{
-  return ram_in_window(ACTION_BASE_ADDRESS,ACTION_MEM_LENGTH,address,length);
-}
-
-void action_process_write_cmd(unsigned short int address,unsigned short int length,unsigned char *data)
-{
-  if(ram_in_range(DARWIN_ACTION_CNTRL,address,length))
-  {
-    if(data[DARWIN_ACTION_CNTRL-address]&ACTION_START)
-      action_start_page();
-    if(data[DARWIN_ACTION_CNTRL-address]&ACTION_STOP)
-      action_stop_page();
-    if(data[DARWIN_ACTION_CNTRL-address]&ACTION_INT_EN)
-      action_enable_int();
-    else
-      action_disable_int();
-  }
-  if(ram_in_range(DARWIN_ACTION_PAGE,address,length))// load the page identifier 
-    action_load_page(data[DARWIN_ACTION_PAGE-address]);
-}
-
diff --git a/src/darwin_kinematics.c b/src/darwin_kinematics.c
deleted file mode 100755
index a60ca577c8c7529d72d3d5d012f44eede9e65e10..0000000000000000000000000000000000000000
--- a/src/darwin_kinematics.c
+++ /dev/null
@@ -1,82 +0,0 @@
-#include "darwin_kinematics.h"
-#include "darwin_math.h"
-#include <math.h>
-
-uint8_t darwin_leg_ik(float *out, float x, float y, float z, float a, float b, float c)
-{
-  TMatrix3D Tad, Tda, Tcd, Tdc, Tac;
-  TVector3D vec,orientation;
-  TPoint3D position;
-  float _Rac, _Acos, _Atan, _k, _l, _m, _n, _s, _c, _theta;
-
-  vector3d_load(&orientation,a * 180.0 / PI, b * 180.0 / PI, c * 180.0 / PI);
-  point3d_load(&position,x, y, z - DARWIN_LEG_LENGTH);
-  matrix3d_set_transform(&Tad,&position,&orientation);
-
-  vec.x = x + Tad.coef[2] * DARWIN_ANKLE_LENGTH;
-  vec.y = y + Tad.coef[6] * DARWIN_ANKLE_LENGTH;
-  vec.z = (z - DARWIN_LEG_LENGTH) + Tad.coef[10] * DARWIN_ANKLE_LENGTH;
-
-  // Get Knee
-  _Rac = vector3d_length(&vec);
-  _Acos = acos((_Rac * _Rac - DARWIN_THIGH_LENGTH * DARWIN_THIGH_LENGTH - DARWIN_CALF_LENGTH * DARWIN_CALF_LENGTH) / (2 * DARWIN_THIGH_LENGTH * DARWIN_CALF_LENGTH));
-  if(isnan(_Acos) == 1)
-    return 0x00;;
-  *(out + 3) = _Acos;
-
-  // Get Ankle Roll
-  Tda = Tad;
-  if(matrix3d_inverse(&Tda,&Tda) == 0x00)
-    return 0x00;
-  _k = sqrt(Tda.coef[7] * Tda.coef[7] + Tda.coef[11] * Tda.coef[11]);
-  _l = sqrt(Tda.coef[7] * Tda.coef[7] + (Tda.coef[11] - DARWIN_ANKLE_LENGTH) * (Tda.coef[11] - DARWIN_ANKLE_LENGTH));
-  _m = (_k * _k - _l * _l - DARWIN_ANKLE_LENGTH * DARWIN_ANKLE_LENGTH) / (2 * _l * DARWIN_ANKLE_LENGTH);
-  if(_m > 1.0)
-    _m = 1.0;
-  else if(_m < -1.0)
-    _m = -1.0;
-  _Acos = acos(_m);
-  if(isnan(_Acos) == 1)
-    return 0x00;
-  if(Tda.coef[7] < 0.0)
-    *(out + 5) = -_Acos;
-  else
-    *(out + 5) = _Acos;
-  // Get Hip Yaw
-  vector3d_load(&orientation,*(out + 5) * 180.0 / PI, 0, 0);
-  point3d_load(&position,0, 0, -DARWIN_ANKLE_LENGTH);
-  matrix3d_set_transform(&Tcd,&position,&orientation);
-  Tdc = Tcd;
-  if(matrix3d_inverse(&Tdc,&Tdc) == 0x00)
-    return 0x00;
-  matrix3d_mult(&Tac,&Tad,&Tdc);
-  _Atan = atan2(-Tac.coef[1] , Tac.coef[5]);
-  if(isinf(_Atan) == 1)
-    return 0x00;
-  *(out) = _Atan;
-
-  // Get Hip Roll
-  _Atan = atan2(Tac.coef[9], -Tac.coef[1] * sin(*(out)) + Tac.coef[5] * cos(*(out)));
-  if(isinf(_Atan) == 1)
-    return 0x00;
-  *(out + 1) = _Atan;
-
-  // Get Hip Pitch and Ankle Pitch
-  _Atan = atan2(Tac.coef[2] * cos(*(out)) + Tac.coef[6] * sin(*(out)), Tac.coef[0] * cos(*(out)) + Tac.coef[4] * sin(*(out)));
-  if(isinf(_Atan) == 1)
-    return 0x00;
-  _theta = _Atan;
-  _k = sin(*(out + 3)) * DARWIN_CALF_LENGTH;
-  _l = -DARWIN_THIGH_LENGTH - cos(*(out + 3)) * DARWIN_CALF_LENGTH;
-  _m = cos(*(out)) * vec.x + sin(*(out)) * vec.y;
-  _n = cos(*(out + 1)) * vec.z + sin(*(out)) * sin(*(out + 1)) * vec.x - cos(*(out)) * sin(*(out + 1)) * vec.y;
-  _s = (_k * _n + _l * _m) / (_k * _k + _l * _l);
-  _c = (_n - _k * _s) / _l;
-  _Atan = atan2(_s, _c);
-  if(isinf(_Atan) == 1)
-    return 0x00;
-  *(out + 2) = _Atan;
-  *(out + 4) = _theta - *(out + 3) - *(out + 2);
-
-  return 0x01;
-}
diff --git a/src/darwin_math.c b/src/darwin_math.c
deleted file mode 100755
index e76646357603c96cf91e2dc5994c9fea00f1971d..0000000000000000000000000000000000000000
--- a/src/darwin_math.c
+++ /dev/null
@@ -1,237 +0,0 @@
-#include "darwin_math.h"
-#include <math.h>
-
-// point functions
-void point3d_init(TPoint3D *point)
-{
-  point->x=0.0;
-  point->y=0.0;
-  point->z=0.0;
-}
-
-void point3d_load(TPoint3D *point, float x, float y,float z)
-{
-  point->x=x;
-  point->y=y;
-  point->z=z;  
-}
-
-void point3d_copy(TPoint3D *point_dst, TPoint3D *point_src)
-{
-  point_dst->x=point_src->x;
-  point_dst->y=point_src->y;
-  point_dst->z=point_src->z;
-}
-
-// vector functions
-void vector3d_init(TVector3D *vector)
-{
-  vector->x=0.0;
-  vector->y=0.0;
-  vector->z=0.0;
-}
-
-void vector3d_load(TVector3D *vector, float x, float y, float z)
-{
-  vector->x=x;
-  vector->y=y;
-  vector->z=z;
-}
-
-void vector3d_copy(TVector3D *vector_dst, TVector3D *vector_src)
-{
-  vector_dst->x=vector_src->x;
-  vector_dst->y=vector_src->y;
-  vector_dst->z=vector_src->z;
-}
-
-float vector3d_length(TVector3D *vector)
-{
-  return sqrt(vector->x*vector->x + vector->y*vector->y + vector->z*vector->z); 
-}
-
-// matrix functions
-void matrix3d_init(TMatrix3D *matrix)
-{
-   matrix3d_identity(matrix);
-}
-
-void matrix3d_copy(TMatrix3D *matrix_dst, TMatrix3D *matrix_src)
-{
-  matrix_dst->coef[m00]=matrix_src->coef[m00];
-  matrix_dst->coef[m01]=matrix_src->coef[m01];
-  matrix_dst->coef[m02]=matrix_src->coef[m02];
-  matrix_dst->coef[m03]=matrix_src->coef[m03];
-  matrix_dst->coef[m10]=matrix_src->coef[m10];
-  matrix_dst->coef[m11]=matrix_src->coef[m11];
-  matrix_dst->coef[m12]=matrix_src->coef[m12];
-  matrix_dst->coef[m13]=matrix_src->coef[m13];
-  matrix_dst->coef[m20]=matrix_src->coef[m20];
-  matrix_dst->coef[m21]=matrix_src->coef[m21];
-  matrix_dst->coef[m22]=matrix_src->coef[m22];
-  matrix_dst->coef[m23]=matrix_src->coef[m23];
-  matrix_dst->coef[m30]=matrix_src->coef[m30];
-  matrix_dst->coef[m31]=matrix_src->coef[m31];
-  matrix_dst->coef[m32]=matrix_src->coef[m32];
-  matrix_dst->coef[m33]=matrix_src->coef[m33];
-}
-
-void matrix3d_zero(TMatrix3D *matrix)
-{
-  matrix->coef[m00]=0.0;
-  matrix->coef[m01]=0.0;
-  matrix->coef[m02]=0.0;
-  matrix->coef[m03]=0.0;
-  matrix->coef[m10]=0.0;
-  matrix->coef[m11]=0.0;
-  matrix->coef[m12]=0.0;
-  matrix->coef[m13]=0.0;
-  matrix->coef[m20]=0.0;
-  matrix->coef[m21]=0.0;
-  matrix->coef[m22]=0.0;
-  matrix->coef[m23]=0.0;
-  matrix->coef[m30]=0.0;
-  matrix->coef[m31]=0.0;
-  matrix->coef[m32]=0.0;
-  matrix->coef[m33]=0.0;
-}
-
-void matrix3d_identity(TMatrix3D *matrix)
-{
-  matrix->coef[m00]=1.0;
-  matrix->coef[m01]=0.0;
-  matrix->coef[m02]=0.0;
-  matrix->coef[m03]=0.0;
-  matrix->coef[m10]=0.0;
-  matrix->coef[m11]=1.0;
-  matrix->coef[m12]=0.0;
-  matrix->coef[m13]=0.0;
-  matrix->coef[m20]=0.0;
-  matrix->coef[m21]=0.0;
-  matrix->coef[m22]=1.0;
-  matrix->coef[m23]=0.0;
-  matrix->coef[m30]=0.0;
-  matrix->coef[m31]=0.0;
-  matrix->coef[m32]=0.0;
-  matrix->coef[m33]=1.0;
-}
-
-uint8_t matrix3d_inverse(TMatrix3D *org, TMatrix3D *inv)
-{
-  TMatrix3D src, tmp;
-  float det;
-  uint8_t i;
-
-  /* transpose matrix */
-  for (i = 0; i < 4; i++)
-  {
-    src.coef[i] = org->coef[i*4];
-    src.coef[i + 4] = org->coef[i*4 + 1];
-    src.coef[i + 8] = org->coef[i*4 + 2];
-    src.coef[i + 12] = org->coef[i*4 + 3];
-  }
-
-  /* calculate pairs for first 8 elements (cofactors) */
-  tmp.coef[0] = src.coef[10] * src.coef[15];
-  tmp.coef[1] = src.coef[11] * src.coef[14];
-  tmp.coef[2] = src.coef[9] * src.coef[15];
-  tmp.coef[3] = src.coef[11] * src.coef[13];
-  tmp.coef[4] = src.coef[9] * src.coef[14];
-  tmp.coef[5] = src.coef[10] * src.coef[13];
-  tmp.coef[6] = src.coef[8] * src.coef[15];
-  tmp.coef[7] = src.coef[11] * src.coef[12];
-  tmp.coef[8] = src.coef[8] * src.coef[14];
-  tmp.coef[9] = src.coef[10] * src.coef[12];
-  tmp.coef[10] = src.coef[8] * src.coef[13];
-  tmp.coef[11] = src.coef[9] * src.coef[12];
-  /* calculate first 8 elements (cofactors) */
-  inv->coef[0] = (tmp.coef[0]*src.coef[5] + tmp.coef[3]*src.coef[6] + tmp.coef[4]*src.coef[7]) - (tmp.coef[1]*src.coef[5] + tmp.coef[2]*src.coef[6] + tmp.coef[5]*src.coef[7]);
-  inv->coef[1] = (tmp.coef[1]*src.coef[4] + tmp.coef[6]*src.coef[6] + tmp.coef[9]*src.coef[7]) - (tmp.coef[0]*src.coef[4] + tmp.coef[7]*src.coef[6] + tmp.coef[8]*src.coef[7]);
-  inv->coef[2] = (tmp.coef[2]*src.coef[4] + tmp.coef[7]*src.coef[5] + tmp.coef[10]*src.coef[7]) - (tmp.coef[3]*src.coef[4] + tmp.coef[6]*src.coef[5] + tmp.coef[11]*src.coef[7]);
-  inv->coef[3] = (tmp.coef[5]*src.coef[4] + tmp.coef[8]*src.coef[5] + tmp.coef[11]*src.coef[6]) - (tmp.coef[4]*src.coef[4] + tmp.coef[9]*src.coef[5] + tmp.coef[10]*src.coef[6]);
-  inv->coef[4] = (tmp.coef[1]*src.coef[1] + tmp.coef[2]*src.coef[2] + tmp.coef[5]*src.coef[3]) - (tmp.coef[0]*src.coef[1] + tmp.coef[3]*src.coef[2] + tmp.coef[4]*src.coef[3]);
-  inv->coef[5] = (tmp.coef[0]*src.coef[0] + tmp.coef[7]*src.coef[2] + tmp.coef[8]*src.coef[3]) - (tmp.coef[1]*src.coef[0] + tmp.coef[6]*src.coef[2] + tmp.coef[9]*src.coef[3]);
-  inv->coef[6] = (tmp.coef[3]*src.coef[0] + tmp.coef[6]*src.coef[1] + tmp.coef[11]*src.coef[3]) - (tmp.coef[2]*src.coef[0] + tmp.coef[7]*src.coef[1] + tmp.coef[10]*src.coef[3]);
-  inv->coef[7] = (tmp.coef[4]*src.coef[0] + tmp.coef[9]*src.coef[1] + tmp.coef[10]*src.coef[2]) - (tmp.coef[5]*src.coef[0] + tmp.coef[8]*src.coef[1] + tmp.coef[11]*src.coef[2]);
-  /* calculate pairs for second 8 elements (cofactors) */
-  tmp.coef[0] = src.coef[2]*src.coef[7];
-  tmp.coef[1] = src.coef[3]*src.coef[6];
-  tmp.coef[2] = src.coef[1]*src.coef[7];
-  tmp.coef[3] = src.coef[3]*src.coef[5];
-  tmp.coef[4] = src.coef[1]*src.coef[6];
-  tmp.coef[5] = src.coef[2]*src.coef[5];
-  //Streaming SIMD Extensions - Inverse of 4x4 Matrix 8
-  tmp.coef[6] = src.coef[0]*src.coef[7];
-  tmp.coef[7] = src.coef[3]*src.coef[4];
-  tmp.coef[8] = src.coef[0]*src.coef[6];
-  tmp.coef[9] = src.coef[2]*src.coef[4];
-  tmp.coef[10] = src.coef[0]*src.coef[5];
-  tmp.coef[11] = src.coef[1]*src.coef[4];
-  /* calculate second 8 elements (cofactors) */
-  inv->coef[8] = (tmp.coef[0]*src.coef[13] + tmp.coef[3]*src.coef[14] + tmp.coef[4]*src.coef[15]) - (tmp.coef[1]*src.coef[13] + tmp.coef[2]*src.coef[14] + tmp.coef[5]*src.coef[15]);
-  inv->coef[9] = (tmp.coef[1]*src.coef[12] + tmp.coef[6]*src.coef[14] + tmp.coef[9]*src.coef[15]) - (tmp.coef[0]*src.coef[12] + tmp.coef[7]*src.coef[14] + tmp.coef[8]*src.coef[15]);
-  inv->coef[10] = (tmp.coef[2]*src.coef[12] + tmp.coef[7]*src.coef[13] + tmp.coef[10]*src.coef[15]) - (tmp.coef[3]*src.coef[12] + tmp.coef[6]*src.coef[13] + tmp.coef[11]*src.coef[15]);
-  inv->coef[11] = (tmp.coef[5]*src.coef[12] + tmp.coef[8]*src.coef[13] + tmp.coef[11]*src.coef[14]) - (tmp.coef[4]*src.coef[12] + tmp.coef[9]*src.coef[13] + tmp.coef[10]*src.coef[14]);
-  inv->coef[12] = (tmp.coef[2]*src.coef[10] + tmp.coef[5]*src.coef[11] + tmp.coef[1]*src.coef[9]) - (tmp.coef[4]*src.coef[11] + tmp.coef[0]*src.coef[9] + tmp.coef[3]*src.coef[10]);
-  inv->coef[13] = (tmp.coef[8]*src.coef[11] + tmp.coef[0]*src.coef[8] + tmp.coef[7]*src.coef[10]) - (tmp.coef[6]*src.coef[10] + tmp.coef[9]*src.coef[11] + tmp.coef[1]*src.coef[8]);
-  inv->coef[14] = (tmp.coef[6]*src.coef[9] + tmp.coef[11]*src.coef[11] + tmp.coef[3]*src.coef[8]) - (tmp.coef[10]*src.coef[11] + tmp.coef[2]*src.coef[8] + tmp.coef[7]*src.coef[9]);
-  inv->coef[15] = (tmp.coef[10]*src.coef[10] + tmp.coef[4]*src.coef[8] + tmp.coef[9]*src.coef[9]) - (tmp.coef[8]*src.coef[9] + tmp.coef[11]*src.coef[10] + tmp.coef[5]*src.coef[8]);
-  /* calculate determinant */
-  det = src.coef[0]*inv->coef[0] + src.coef[1]*inv->coef[1] + src.coef[2]*inv->coef[2] + src.coef[3]*inv->coef[3];
-  /* calculate matrix inverse */
-  if (det == 0)
-  {
-    det = 0;
-    return 0x00;
-  }
-  else
-  {
-    det = 1 / det;
-  }
-
-  for (i = 0; i < 16; i++)
-    inv->coef[i]*=det;
-
-  return 0x01;
-}
-
-void matrix3d_set_transform(TMatrix3D *matrix, TPoint3D *point, TVector3D *vector)
-{
-  float Cx = cos(vector->x * 3.141592 / 180.0);
-  float Cy = cos(vector->y * 3.141592 / 180.0);
-  float Cz = cos(vector->z * 3.141592 / 180.0);
-  float Sx = sin(vector->x * 3.141592 / 180.0);
-  float Sy = sin(vector->y * 3.141592 / 180.0);
-  float Sz = sin(vector->z * 3.141592 / 180.0);
-
-  matrix3d_identity(matrix);
-  matrix->coef[0] = Cz * Cy;
-  matrix->coef[1] = Cz * Sy * Sx - Sz * Cx;
-  matrix->coef[2] = Cz * Sy * Cx + Sz * Sx;
-  matrix->coef[3] = point->x;
-  matrix->coef[4] = Sz * Cy;
-  matrix->coef[5] = Sz * Sy * Sx + Cz * Cx;
-  matrix->coef[6] = Sz * Sy * Cx - Cz * Sx;
-  matrix->coef[7] = point->y;
-  matrix->coef[8] = -Sy;
-  matrix->coef[9] = Cy * Sx;
-  matrix->coef[10] = Cy * Cx;
-  matrix->coef[11] = point->z;
-}
-
-void matrix3d_mult(TMatrix3D *dst,TMatrix3D *a,TMatrix3D *b)
-{
-  uint8_t i,j,c;
-
-  matrix3d_zero(dst);
-  for(j = 0; j < 4; j++)
-  {
-    for(i = 0; i < 4; i++)
-    {
-      for(c = 0; c < 4; c++)
-      {
-         dst->coef[j*4+i] += a->coef[j*4+c] * b->coef[c*4+i];
-      }
-    }
-  }
-}
diff --git a/src/grippers.c b/src/grippers.c
deleted file mode 100644
index 14ab402f9e7c8597bb42e27e0d09349dda066e7f..0000000000000000000000000000000000000000
--- a/src/grippers.c
+++ /dev/null
@@ -1,218 +0,0 @@
-#include "darwin_dyn_master_v2.h"
-#include "dyn_servos.h"
-#include "grippers.h"
-#include "ram.h"
-#include "joint_motion.h"
-
-// private variables
-int64_t grippers_period;
-int16_t grippers_period_us;
-uint8_t gripper_left_target;
-uint8_t gripper_right_target;
-
-// public functions
-void grippers_init(uint16_t period_us)
-{
-  uint16_t force;
-  uint32_t servo_mask=0x00000000;
-
-  /* initialize private variables */
-  gripper_left_target=0x00;//close;
-  gripper_right_target=0x00;//close;
-  grippers_period=(period_us<<16)/1000000;
-  grippers_period_us=period_us;
-  /* configure the maximum force of the servos */
-  servo_mask=0x00000000;
-  if(ram_data[DARWIN_GRIPPER_LEFT_TOP_ID]!=0xFF && ram_data[DARWIN_GRIPPER_LEFT_BOT_ID]!=0xFF)
-  {
-    force=ram_data[DARWIN_GRIPPER_LEFT_MAX_FORCE_L]+(ram_data[DARWIN_GRIPPER_LEFT_MAX_FORCE_H]<<8);
-    dyn_master_write_word(&darwin_dyn_master_v2,ram_data[DARWIN_GRIPPER_LEFT_TOP_ID],XL_TORQUE_LIMIT_L,force);
-    dyn_master_write_word(&darwin_dyn_master_v2,ram_data[DARWIN_GRIPPER_LEFT_BOT_ID],XL_TORQUE_LIMIT_L,force);
-    servo_mask|=1<<ram_data[DARWIN_GRIPPER_LEFT_TOP_ID];
-    servo_mask|=1<<ram_data[DARWIN_GRIPPER_LEFT_BOT_ID];
-    joint_motion_set_group_servos(1,servo_mask);
-  }
-  servo_mask=0x00000000;
-  if(ram_data[DARWIN_GRIPPER_RIGHT_TOP_ID]!=0xFF && ram_data[DARWIN_GRIPPER_RIGHT_BOT_ID]!=0xFF)
-  {
-    force=ram_data[DARWIN_GRIPPER_RIGHT_MAX_FORCE_L]+(ram_data[DARWIN_GRIPPER_RIGHT_MAX_FORCE_H]<<8);
-    dyn_master_write_word(&darwin_dyn_master_v2,ram_data[DARWIN_GRIPPER_RIGHT_TOP_ID],XL_TORQUE_LIMIT_L,force);
-    dyn_master_write_word(&darwin_dyn_master_v2,ram_data[DARWIN_GRIPPER_RIGHT_BOT_ID],XL_TORQUE_LIMIT_L,force);
-    servo_mask|=1<<ram_data[DARWIN_GRIPPER_RIGHT_TOP_ID];
-    servo_mask|=1<<ram_data[DARWIN_GRIPPER_RIGHT_BOT_ID];
-    joint_motion_set_group_servos(2,servo_mask);
-  }
-  grippers_close(GRIPPER_LEFT);
-  grippers_close(GRIPPER_RIGHT);
-}
-
-void grippers_set_period(uint16_t period_us)
-{
-  grippers_period=(period_us<<16)/1000000;
-  grippers_period_us=period_us;
-}
-
-uint16_t grippers_get_period(void)
-{
-  return grippers_period_us;
-}
-
-void grippers_open(grippers_t gripper)
-{
-  if(gripper==GRIPPER_LEFT)
-  {
-    ram_data[DARWIN_JOINT_SERVO0_ANGLE_L+ram_data[DARWIN_GRIPPER_LEFT_TOP_ID]*2]=ram_data[DARWIN_GRIPPER_RIGHT_MAX_ANGLE_L];
-    ram_data[DARWIN_JOINT_SERVO0_ANGLE_L+ram_data[DARWIN_GRIPPER_LEFT_TOP_ID]*2+1]=ram_data[DARWIN_GRIPPER_RIGHT_MAX_ANGLE_H];
-    ram_data[DARWIN_JOINT_SERVO0_SPEED+ram_data[DARWIN_GRIPPER_LEFT_TOP_ID]]=60;
-    ram_data[DARWIN_JOINT_SERVO0_ANGLE_L+ram_data[DARWIN_GRIPPER_LEFT_BOT_ID]*2]=ram_data[DARWIN_GRIPPER_RIGHT_MAX_ANGLE_L];
-    ram_data[DARWIN_JOINT_SERVO0_ANGLE_L+ram_data[DARWIN_GRIPPER_LEFT_BOT_ID]*2+1]=ram_data[DARWIN_GRIPPER_RIGHT_MAX_ANGLE_H];
-    ram_data[DARWIN_JOINT_SERVO0_SPEED+ram_data[DARWIN_GRIPPER_LEFT_BOT_ID]]=60;
-    joint_motion_start(1);
-  }
-  else if(gripper==GRIPPER_RIGHT)
-  {
-    ram_data[DARWIN_JOINT_SERVO0_ANGLE_L+ram_data[DARWIN_GRIPPER_RIGHT_TOP_ID]*2]=ram_data[DARWIN_GRIPPER_RIGHT_MAX_ANGLE_L];
-    ram_data[DARWIN_JOINT_SERVO0_ANGLE_L+ram_data[DARWIN_GRIPPER_RIGHT_TOP_ID]*2+1]=ram_data[DARWIN_GRIPPER_RIGHT_MAX_ANGLE_H];
-    ram_data[DARWIN_JOINT_SERVO0_SPEED+ram_data[DARWIN_GRIPPER_RIGHT_TOP_ID]]=60;
-    ram_data[DARWIN_JOINT_SERVO0_ANGLE_L+ram_data[DARWIN_GRIPPER_RIGHT_BOT_ID]*2]=ram_data[DARWIN_GRIPPER_RIGHT_MAX_ANGLE_L];
-    ram_data[DARWIN_JOINT_SERVO0_ANGLE_L+ram_data[DARWIN_GRIPPER_RIGHT_BOT_ID]*2+1]=ram_data[DARWIN_GRIPPER_RIGHT_MAX_ANGLE_H];
-    ram_data[DARWIN_JOINT_SERVO0_SPEED+ram_data[DARWIN_GRIPPER_RIGHT_BOT_ID]]=60;
-    joint_motion_start(2);
-  }
-}
-
-void grippers_close(grippers_t gripper)
-{
-  if(gripper==GRIPPER_LEFT)
-  {
-    ram_data[DARWIN_JOINT_SERVO0_ANGLE_L+ram_data[DARWIN_GRIPPER_LEFT_TOP_ID]*2]=ram_data[DARWIN_GRIPPER_RIGHT_MIN_ANGLE_L];
-    ram_data[DARWIN_JOINT_SERVO0_ANGLE_L+ram_data[DARWIN_GRIPPER_LEFT_TOP_ID]*2+1]=ram_data[DARWIN_GRIPPER_RIGHT_MIN_ANGLE_H];
-    ram_data[DARWIN_JOINT_SERVO0_SPEED+ram_data[DARWIN_GRIPPER_LEFT_TOP_ID]]=60;
-    ram_data[DARWIN_JOINT_SERVO0_ANGLE_L+ram_data[DARWIN_GRIPPER_LEFT_BOT_ID]*2]=ram_data[DARWIN_GRIPPER_RIGHT_MIN_ANGLE_L];
-    ram_data[DARWIN_JOINT_SERVO0_ANGLE_L+ram_data[DARWIN_GRIPPER_LEFT_BOT_ID]*2+1]=ram_data[DARWIN_GRIPPER_RIGHT_MIN_ANGLE_H];
-    ram_data[DARWIN_JOINT_SERVO0_SPEED+ram_data[DARWIN_GRIPPER_LEFT_BOT_ID]]=60;
-    joint_motion_start(1);
-  }
-  else if(gripper==GRIPPER_RIGHT)
-  {
-    ram_data[DARWIN_JOINT_SERVO0_ANGLE_L+ram_data[DARWIN_GRIPPER_RIGHT_TOP_ID]*2]=ram_data[DARWIN_GRIPPER_RIGHT_MIN_ANGLE_L];
-    ram_data[DARWIN_JOINT_SERVO0_ANGLE_L+ram_data[DARWIN_GRIPPER_RIGHT_TOP_ID]*2+1]=ram_data[DARWIN_GRIPPER_RIGHT_MIN_ANGLE_H];
-    ram_data[DARWIN_JOINT_SERVO0_SPEED+ram_data[DARWIN_GRIPPER_RIGHT_TOP_ID]]=60;
-    ram_data[DARWIN_JOINT_SERVO0_ANGLE_L+ram_data[DARWIN_GRIPPER_RIGHT_BOT_ID]*2]=ram_data[DARWIN_GRIPPER_RIGHT_MIN_ANGLE_L];
-    ram_data[DARWIN_JOINT_SERVO0_ANGLE_L+ram_data[DARWIN_GRIPPER_RIGHT_BOT_ID]*2+1]=ram_data[DARWIN_GRIPPER_RIGHT_MIN_ANGLE_H];
-    ram_data[DARWIN_JOINT_SERVO0_SPEED+ram_data[DARWIN_GRIPPER_RIGHT_BOT_ID]]=60;
-    joint_motion_start(2);
-  }
-}
-
-void grippers_stop(grippers_t gripper)
-{
-  if(gripper==GRIPPER_LEFT)
-    joint_motion_stop(1);
-  else
-    joint_motion_stop(2);
-}
-
-uint8_t gripper_is_open(grippers_t gripper)
-{
-  if(gripper==GRIPPER_LEFT)
-  {
-    if(ram_data[DARWIN_GRIPPER_CNTRL]&GRIPPER_OPENED_LEFT)
-      return 0x01;
-    else
-      return 0x00;
-  }
-  else
-  {
-    if(ram_data[DARWIN_GRIPPER_CNTRL]&GRIPPER_OPENED_RIGHT)
-      return 0x01;
-    else
-      return 0x00;
-  }
-}
-
-uint8_t gripper_is_close(grippers_t gripper)
-{
-  if(gripper==GRIPPER_LEFT)
-  {
-    if(ram_data[DARWIN_GRIPPER_CNTRL]&GRIPPER_OPENED_LEFT)
-      return 0x00;
-    else
-      return 0x01;
-  }
-  else
-  {
-    if(ram_data[DARWIN_GRIPPER_CNTRL]&GRIPPER_OPENED_RIGHT)
-      return 0x00;
-    else
-      return 0x01;
-  }
-}
-
-uint8_t gripper_is_moving(grippers_t gripper)
-{
-  if(gripper==GRIPPER_LEFT)
-    return are_joints_moving(1);
-  else
-    return are_joints_moving(2);
-}
-
-// operation functions
-uint8_t grippers_in_range(unsigned short int address, unsigned short int length)
-{
-  if(ram_in_window(GRIPPER_BASE_ADDRESS,GRIPPER_MEM_LENGTH,address,length) ||
-     ram_in_window(GRIPPER_EEPROM_ADDRESS,GRIPPER_EEPROM_LENGTH,address,length))
-    return 0x01;
-  else
-    return 0x00;
-}
-
-void grippers_process_read_cmd(unsigned short int address,unsigned short int length,unsigned char *data)
-{
-
-}
-
-void grippers_process_write_cmd(unsigned short int address,unsigned short int length,unsigned char *data)
-{
-  if(ram_in_range(DARWIN_GRIPPER_CNTRL,address,length))
-  {
-    if(data[DARWIN_GRIPPER_CNTRL-address]&GRIPPER_OPEN_RIGHT)
-      grippers_open(GRIPPER_RIGHT);
-    else if(data[DARWIN_GRIPPER_CNTRL-address]&GRIPPER_CLOSE_RIGHT)
-      grippers_close(GRIPPER_RIGHT);
-    if(data[DARWIN_GRIPPER_CNTRL-address]&GRIPPER_OPEN_LEFT)
-      grippers_open(GRIPPER_LEFT);
-    else if(data[DARWIN_GRIPPER_CNTRL-address]&GRIPPER_CLOSE_LEFT)
-      grippers_close(GRIPPER_LEFT);
-  }
-}
-
-void grippers_process(void)
-{
-  if(!are_joints_moving(1))
-  {
-    ram_data[DARWIN_GRIPPER_CNTRL]&=~GRIPPER_MOVING_LEFT;
-    if(gripper_left_target==0x01)
-      ram_data[DARWIN_GRIPPER_CNTRL]|=GRIPPER_OPENED_LEFT;
-    else
-      ram_data[DARWIN_GRIPPER_CNTRL]&=~GRIPPER_OPENED_LEFT;
-  }
-  else
-  {
-    ram_data[DARWIN_GRIPPER_CNTRL]&=~GRIPPER_OPENED_LEFT;
-    ram_data[DARWIN_GRIPPER_CNTRL]|=GRIPPER_MOVING_LEFT;
-  }
-  if(!are_joints_moving(2))
-  {
-    ram_data[DARWIN_GRIPPER_CNTRL]&=~GRIPPER_MOVING_RIGHT;
-    if(gripper_right_target==0x01)
-      ram_data[DARWIN_GRIPPER_CNTRL]|=GRIPPER_OPENED_RIGHT;
-    else
-      ram_data[DARWIN_GRIPPER_CNTRL]&=~GRIPPER_OPENED_RIGHT;
-  }
-  else
-  {
-    ram_data[DARWIN_GRIPPER_CNTRL]&=~GRIPPER_OPENED_RIGHT;
-    ram_data[DARWIN_GRIPPER_CNTRL]|=GRIPPER_MOVING_RIGHT;
-  }
-}
diff --git a/src/head_tracking.c b/src/head_tracking.c
deleted file mode 100644
index 12ee27c4d4485534af863f89d724864df4104f16..0000000000000000000000000000000000000000
--- a/src/head_tracking.c
+++ /dev/null
@@ -1,333 +0,0 @@
-#include "head_tracking.h"
-#include "ram.h"
-
-// private variables
-typedef struct{
-  uint16_t kp;
-  uint16_t ki;
-  uint16_t kd;
-  int64_t prev_error;// 48|16
-  int64_t integral_part;
-  int64_t integral_clamp;
-  int64_t max_angle;
-  int64_t min_angle;
-  int64_t target_angle;
-}TJointControl;
-
-TJointControl head_tracking_pan;
-TJointControl head_tracking_tilt;
-int64_t head_tracking_period;
-int16_t head_tracking_period_us;
-uint8_t head_tracking_enabled;
-
-// public functions
-void head_tracking_init(uint16_t period_us)
-{
-  /* initialize private variables */
-  head_tracking_pan.kp=ram_data[DARWIN_HEAD_PAN_P_L]+(ram_data[DARWIN_HEAD_PAN_P_H]<<8);
-  head_tracking_pan.ki=ram_data[DARWIN_HEAD_PAN_I_L]+(ram_data[DARWIN_HEAD_PAN_I_H]<<8);
-  head_tracking_pan.kd=ram_data[DARWIN_HEAD_PAN_D_L]+(ram_data[DARWIN_HEAD_PAN_D_H]<<8);
-  head_tracking_pan.prev_error=0;
-  head_tracking_pan.integral_part=0;
-  head_tracking_pan.integral_clamp=(ram_data[DARWIN_HEAD_PAN_I_CLAMP_L]+(ram_data[DARWIN_HEAD_PAN_I_CLAMP_H]<<8))<<9;
-  head_tracking_pan.min_angle=manager_get_cw_angle_limit(L_PAN)<<9;
-  ram_data[DARWIN_HEAD_MIN_PAN_L]=manager_get_cw_angle_limit(L_PAN)%256;
-  ram_data[DARWIN_HEAD_MIN_PAN_H]=manager_get_cw_angle_limit(L_PAN)/256;
-  head_tracking_pan.max_angle=manager_get_ccw_angle_limit(L_PAN)<<9;
-  ram_data[DARWIN_HEAD_MAX_PAN_L]=manager_get_ccw_angle_limit(L_PAN)%256;
-  ram_data[DARWIN_HEAD_MAX_PAN_H]=manager_get_ccw_angle_limit(L_PAN)/256;
-  head_tracking_pan.target_angle=0;
-  head_tracking_tilt.kp=ram_data[DARWIN_HEAD_TILT_P_L]+(ram_data[DARWIN_HEAD_TILT_P_H]<<8);
-  head_tracking_tilt.ki=ram_data[DARWIN_HEAD_TILT_I_L]+(ram_data[DARWIN_HEAD_TILT_I_H]<<8);
-  head_tracking_tilt.kd=ram_data[DARWIN_HEAD_TILT_D_L]+(ram_data[DARWIN_HEAD_TILT_D_H]<<8);
-  head_tracking_tilt.prev_error=0;
-  head_tracking_tilt.integral_part=0;
-  head_tracking_tilt.integral_clamp=(ram_data[DARWIN_HEAD_TILT_I_CLAMP_L]+(ram_data[DARWIN_HEAD_TILT_I_CLAMP_H]<<8))<<9;
-  head_tracking_tilt.min_angle=manager_get_cw_angle_limit(L_TILT)<<9;
-  ram_data[DARWIN_HEAD_MIN_TILT_L]=manager_get_cw_angle_limit(L_TILT)%256;
-  ram_data[DARWIN_HEAD_MIN_TILT_H]=manager_get_cw_angle_limit(L_TILT)/256;
-  head_tracking_tilt.max_angle=manager_get_ccw_angle_limit(L_TILT)<<9;
-  ram_data[DARWIN_HEAD_MAX_TILT_L]=manager_get_ccw_angle_limit(L_TILT)%256;
-  ram_data[DARWIN_HEAD_MAX_TILT_H]=manager_get_ccw_angle_limit(L_TILT)/256;
-  head_tracking_tilt.target_angle=0;
-  head_tracking_period=(period_us<<16)/1000000;
-  head_tracking_period_us=period_us;
-  head_tracking_enabled=0x00;
-}
-
-void head_tracking_set_period(uint16_t period_us)
-{
-  head_tracking_period=(period_us<<16)/1000000;
-  head_tracking_period_us=period_us;
-}
-
-uint16_t head_tracking_get_period(void)
-{
-  return head_tracking_period_us;
-}
-
-void head_tracking_start(void)
-{
-  head_tracking_enabled=0x01;
-  ram_data[DARWIN_HEAD_CNTRL]|=HEAD_STATUS;
-}
-
-void head_tracking_stop(void)
-{
-  head_tracking_enabled=0x00;
-  ram_data[DARWIN_HEAD_CNTRL]&=(~HEAD_STATUS);
-}
-
-uint8_t head_is_tracking(void)
-{
-  return head_tracking_enabled;
-}
-
-void head_tracking_set_pan_range(int16_t max_angle,int16_t min_angle)
-{
-  head_tracking_pan.max_angle=max_angle<<9;
-  ram_data[DARWIN_HEAD_MAX_PAN_L]=max_angle%256;
-  ram_data[DARWIN_HEAD_MAX_PAN_H]=max_angle/256;
-  head_tracking_pan.min_angle=min_angle<<9;
-  ram_data[DARWIN_HEAD_MIN_PAN_L]=min_angle%256;
-  ram_data[DARWIN_HEAD_MIN_PAN_H]=min_angle/256;
-}
-
-void head_tracking_get_pan_range(int16_t *max_angle,int16_t *min_angle)
-{
-  *max_angle=head_tracking_pan.max_angle>>9;
-  *min_angle=head_tracking_pan.min_angle>>9;
-}
-
-void head_tracking_set_pan_pid(uint16_t kp,uint16_t ki,uint16_t kd, uint16_t i_clamp)
-{
-  head_tracking_pan.kp=kp;
-  ram_data[DARWIN_HEAD_PAN_P_L]=kp%256;
-  ram_data[DARWIN_HEAD_PAN_P_H]=kp/256;
-  head_tracking_pan.ki=ki;
-  ram_data[DARWIN_HEAD_PAN_I_L]=ki%256;
-  ram_data[DARWIN_HEAD_PAN_I_H]=ki/256;
-  head_tracking_pan.kd=kd;
-  ram_data[DARWIN_HEAD_PAN_D_L]=kd%256;
-  ram_data[DARWIN_HEAD_PAN_D_H]=kd/256;
-  head_tracking_pan.integral_clamp=i_clamp<<9;
-  ram_data[DARWIN_HEAD_PAN_I_CLAMP_L]=i_clamp%256;
-  ram_data[DARWIN_HEAD_PAN_I_CLAMP_H]=i_clamp/256;
-}
-
-void head_tracking_get_pan_pid(uint16_t *kp,uint16_t *ki,uint16_t *kd, uint16_t *i_clamp)
-{
-  *kp=head_tracking_pan.kp;
-  *ki=head_tracking_pan.ki;
-  *kd=head_tracking_pan.kd;
-  *i_clamp=head_tracking_pan.integral_clamp>>9;
-}
-
-void head_tracking_set_tilt_range(int16_t max_angle,int16_t min_angle)
-{
-  head_tracking_tilt.max_angle=max_angle<<9;
-  ram_data[DARWIN_HEAD_MAX_TILT_L]=max_angle%256;
-  ram_data[DARWIN_HEAD_MAX_TILT_H]=max_angle/256;
-  head_tracking_tilt.min_angle=min_angle<<9;
-  ram_data[DARWIN_HEAD_MIN_TILT_L]=min_angle%256;
-  ram_data[DARWIN_HEAD_MIN_TILT_H]=min_angle/256;
-}
-
-void head_tracking_get_tilt_range(int16_t *max_angle,int16_t *min_angle)
-{
-  *max_angle=head_tracking_tilt.max_angle>>9;
-  *min_angle=head_tracking_tilt.min_angle>>9;
-}
-
-void head_tracking_set_tilt_pid(uint16_t kp,uint16_t ki,uint16_t kd, uint16_t i_clamp)
-{
-  head_tracking_tilt.kp=kp;
-  ram_data[DARWIN_HEAD_TILT_P_L]=kp%256;
-  ram_data[DARWIN_HEAD_TILT_P_H]=kp/256;
-  head_tracking_tilt.ki=ki;
-  ram_data[DARWIN_HEAD_TILT_I_L]=ki%256;
-  ram_data[DARWIN_HEAD_TILT_I_H]=ki/256;
-  head_tracking_tilt.kd=kd;
-  ram_data[DARWIN_HEAD_TILT_D_L]=kd%256;
-  ram_data[DARWIN_HEAD_TILT_D_H]=kd/256;
-  head_tracking_tilt.integral_clamp=i_clamp<<9;
-  ram_data[DARWIN_HEAD_TILT_I_CLAMP_L]=i_clamp%256;
-  ram_data[DARWIN_HEAD_TILT_I_CLAMP_H]=i_clamp/256;
-}
-
-void head_tracking_get_tilt_pid(uint16_t *kp,uint16_t *ki,uint16_t *kd, uint16_t *i_clamp)
-{
-  *kp=head_tracking_tilt.kp;
-  *ki=head_tracking_tilt.ki;
-  *kd=head_tracking_tilt.kd;
-  *i_clamp=head_tracking_tilt.integral_clamp>>9;
-}
-
-void head_tracking_set_target_pan(int16_t target)
-{
-  head_tracking_pan.target_angle=target<<9;
-  ram_data[DARWIN_HEAD_PAN_TARGET_L]=target%256;
-  ram_data[DARWIN_HEAD_PAN_TARGET_H]=target/256;
-}
-
-void head_tracking_set_target_tilt(int16_t target)
-{
-  head_tracking_tilt.target_angle=target<<9;
-  ram_data[DARWIN_HEAD_TILT_TARGET_L]=target%256;
-  ram_data[DARWIN_HEAD_TILT_TARGET_H]=target/256;
-}
-
-// operation functions
-uint8_t head_tracking_in_range(unsigned short int address, unsigned short int length)
-{
-  if(ram_in_window(HEAD_BASE_ADDRESS,HEAD_MEM_LENGTH,address,length) ||
-     ram_in_window(HEAD_EEPROM_ADDRESS,HEAD_EEPROM_LENGTH,address,length))
-    return 0x01;
-  else
-    return 0x00;
-}
-
-void head_tracking_process_read_cmd(unsigned short int address,unsigned short int length,unsigned char *data)
-{
-
-}
-
-void head_tracking_process_write_cmd(unsigned short int address,unsigned short int length,unsigned char *data)
-{
-  uint16_t kp,kd,ki,i_clamp;
-  int16_t max,min,target;
-
-  // head tracking control
-  if(ram_in_range(DARWIN_HEAD_CNTRL,address,length))
-  {
-    if(data[DARWIN_HEAD_CNTRL-address]&HEAD_START)
-      head_tracking_start();
-    if(data[DARWIN_HEAD_CNTRL-address]&HEAD_STOP)
-      head_tracking_stop();
-  }
-  if(ram_in_window(DARWIN_HEAD_PAN_P_L,8,address,length))
-  {
-    head_tracking_get_pan_pid(&kp,&kd,&ki,&i_clamp);
-    if(ram_in_range(DARWIN_HEAD_PAN_P_L,address,length)) kp=(kp&0xFF00)|data[DARWIN_HEAD_PAN_P_L-address];
-    if(ram_in_range(DARWIN_HEAD_PAN_P_H,address,length)) kp=(kp&0x00FF)|(data[DARWIN_HEAD_PAN_P_H-address]<<8);
-    if(ram_in_range(DARWIN_HEAD_PAN_I_L,address,length)) ki=(ki&0xFF00)|data[DARWIN_HEAD_PAN_I_L-address];
-    if(ram_in_range(DARWIN_HEAD_PAN_I_H,address,length)) ki=(ki&0x00FF)|(data[DARWIN_HEAD_PAN_I_H-address]<<8);
-    if(ram_in_range(DARWIN_HEAD_PAN_D_L,address,length)) kd=(kd&0xFF00)|data[DARWIN_HEAD_PAN_D_L-address];
-    if(ram_in_range(DARWIN_HEAD_PAN_D_H,address,length)) kd=(kd&0x00FF)|(data[DARWIN_HEAD_PAN_D_H-address]<<8);
-    if(ram_in_range(DARWIN_HEAD_PAN_I_CLAMP_L,address,length)) i_clamp=(i_clamp&0xFF00)|data[DARWIN_HEAD_PAN_I_CLAMP_L-address];
-    if(ram_in_range(DARWIN_HEAD_PAN_I_CLAMP_H,address,length)) i_clamp=(i_clamp&0x00FF)|(data[DARWIN_HEAD_PAN_I_CLAMP_H-address]<<8);
-    head_tracking_set_pan_pid(kp,kd,ki,i_clamp);
-  }
-  if(ram_in_window(DARWIN_HEAD_TILT_P_L,8,address,length))
-  {
-    head_tracking_get_tilt_pid(&kp,&kd,&ki,&i_clamp);
-    if(ram_in_range(DARWIN_HEAD_TILT_P_L,address,length)) kp=(kp&0xFF00)|data[DARWIN_HEAD_TILT_P_L-address];
-    if(ram_in_range(DARWIN_HEAD_TILT_P_H,address,length)) kp=(kp&0x00FF)|(data[DARWIN_HEAD_TILT_P_H-address]<<8);
-    if(ram_in_range(DARWIN_HEAD_TILT_I_L,address,length)) ki=(ki&0xFF00)|data[DARWIN_HEAD_TILT_I_L-address];
-    if(ram_in_range(DARWIN_HEAD_TILT_I_H,address,length)) ki=(ki&0x00FF)|(data[DARWIN_HEAD_TILT_I_H-address]<<8);
-    if(ram_in_range(DARWIN_HEAD_TILT_D_L,address,length)) kd=(kd&0xFF00)|data[DARWIN_HEAD_TILT_D_L-address];
-    if(ram_in_range(DARWIN_HEAD_TILT_D_H,address,length)) kd=(kd&0x00FF)|(data[DARWIN_HEAD_TILT_D_H-address]<<8);
-    if(ram_in_range(DARWIN_HEAD_TILT_I_CLAMP_L,address,length)) i_clamp=(i_clamp&0xFF00)|data[DARWIN_HEAD_TILT_I_CLAMP_L-address];
-    if(ram_in_range(DARWIN_HEAD_TILT_I_CLAMP_H,address,length)) i_clamp=(i_clamp&0x00FF)|(data[DARWIN_HEAD_TILT_I_CLAMP_H-address]<<8);
-    head_tracking_set_tilt_pid(kp,kd,ki,i_clamp);
-  }
-  if(ram_in_window(DARWIN_HEAD_MAX_PAN_L,4,address,length))
-  {
-    head_tracking_get_pan_range(&max,&min);
-    if(ram_in_range(DARWIN_HEAD_MAX_PAN_L,address,length)) max=(max&0xFF00)|data[DARWIN_HEAD_MAX_PAN_L-address];
-    if(ram_in_range(DARWIN_HEAD_MAX_PAN_H,address,length)) max=(max&0x00FF)|(data[DARWIN_HEAD_MAX_PAN_H-address]<<8);
-    if(ram_in_range(DARWIN_HEAD_MIN_PAN_L,address,length)) min=(min&0xFF00)|data[DARWIN_HEAD_MIN_PAN_L-address];
-    if(ram_in_range(DARWIN_HEAD_MIN_PAN_H,address,length)) min=(min&0x00FF)|(data[DARWIN_HEAD_MIN_PAN_H-address]<<8);
-    head_tracking_set_pan_range(max,min);
-  }
-  if(ram_in_window(DARWIN_HEAD_MAX_TILT_L,4,address,length))
-  {
-    head_tracking_get_tilt_range(&max,&min);
-    if(ram_in_range(DARWIN_HEAD_MAX_TILT_L,address,length)) max=(max&0xFF00)|data[DARWIN_HEAD_MAX_TILT_L-address];
-    if(ram_in_range(DARWIN_HEAD_MAX_TILT_H,address,length)) max=(max&0x00FF)|(data[DARWIN_HEAD_MAX_TILT_H-address]<<8);
-    if(ram_in_range(DARWIN_HEAD_MIN_TILT_L,address,length)) min=(min&0xFF00)|data[DARWIN_HEAD_MIN_TILT_L-address];
-    if(ram_in_range(DARWIN_HEAD_MIN_TILT_H,address,length)) min=(min&0x00FF)|(data[DARWIN_HEAD_MIN_TILT_H-address]<<8);
-    head_tracking_set_tilt_range(max,min);
-  }
-  if(ram_in_window(DARWIN_HEAD_PAN_TARGET_L,2,address,length))
-  {
-    target=(head_tracking_pan.target_angle>>9);
-    if(ram_in_range(DARWIN_HEAD_PAN_TARGET_L,address,length))
-    { 
-      target=(target&0xFF00)|data[DARWIN_HEAD_PAN_TARGET_L-address];
-      ram_data[DARWIN_HEAD_PAN_TARGET_L]=data[DARWIN_HEAD_PAN_TARGET_L-address];
-    }
-    if(ram_in_range(DARWIN_HEAD_PAN_TARGET_H,address,length))
-    {
-      target=(target&0x00FF)|(data[DARWIN_HEAD_PAN_TARGET_H-address]<<8);
-      ram_data[DARWIN_HEAD_PAN_TARGET_H]=data[DARWIN_HEAD_PAN_TARGET_H-address];
-    }
-    head_tracking_pan.target_angle=(target<<9);
-  }
-  if(ram_in_window(DARWIN_HEAD_TILT_TARGET_L,2,address,length))
-  {
-    target=(head_tracking_tilt.target_angle>>9);
-    if(ram_in_range(DARWIN_HEAD_TILT_TARGET_L,address,length))
-    {
-      target=(target&0xFF00)|data[DARWIN_HEAD_TILT_TARGET_L-address];
-      ram_data[DARWIN_HEAD_TILT_TARGET_L]=data[DARWIN_HEAD_TILT_TARGET_L-address];
-    }
-    if(ram_in_range(DARWIN_HEAD_TILT_TARGET_H,address,length))
-    {
-      target=(target&0x00FF)|(data[DARWIN_HEAD_TILT_TARGET_H-address]<<8);
-      ram_data[DARWIN_HEAD_TILT_TARGET_H]=data[DARWIN_HEAD_TILT_TARGET_H-address];
-    }
-    head_tracking_tilt.target_angle=(target<<9);
-  }
-}
-
-// 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_data[DARWIN_HEAD_CNTRL]|=HEAD_STATUS;
-    if(manager_get_module(L_PAN)==MM_HEAD)
-    {
-      pan_error = head_tracking_pan.target_angle-manager_current_angles[L_PAN];
-      head_tracking_pan.integral_part+=(pan_error*head_tracking_period)>>16;
-      if(head_tracking_pan.integral_part>head_tracking_pan.integral_clamp)
-        head_tracking_pan.integral_part=head_tracking_pan.integral_clamp;
-      else if(head_tracking_pan.integral_part<-head_tracking_pan.integral_clamp)
-        head_tracking_pan.integral_part=-head_tracking_pan.integral_clamp;
-      derivative_pan = ((pan_error - head_tracking_pan.prev_error)<<16)/head_tracking_period;
-      manager_current_angles[L_PAN]+=((head_tracking_pan.kp*pan_error)>>16);
-      manager_current_angles[L_PAN]+=((head_tracking_pan.ki*head_tracking_pan.integral_part)>>16);
-      manager_current_angles[L_PAN]+=((head_tracking_pan.kd*derivative_pan)>>16);
-      if(manager_current_angles[L_PAN]>head_tracking_pan.max_angle)
-        manager_current_angles[L_PAN]=head_tracking_pan.max_angle;
-      else if(manager_current_angles[L_PAN]<head_tracking_pan.min_angle)
-        manager_current_angles[L_PAN]=head_tracking_pan.min_angle;
-      head_tracking_pan.prev_error = pan_error;
-    }
-    if(manager_get_module(L_TILT)==MM_HEAD)
-    { 
-      tilt_error = head_tracking_tilt.target_angle-manager_current_angles[L_TILT];
-      head_tracking_tilt.integral_part+=(tilt_error*head_tracking_period)>>16;
-      if(head_tracking_tilt.integral_part>head_tracking_tilt.integral_clamp)
-        head_tracking_tilt.integral_part=head_tracking_tilt.integral_clamp;
-      else if(head_tracking_tilt.integral_part<-head_tracking_tilt.integral_clamp)
-        head_tracking_tilt.integral_part=-head_tracking_tilt.integral_clamp;
-      derivative_tilt = ((tilt_error - head_tracking_tilt.prev_error)<<16)/head_tracking_period;
-      manager_current_angles[L_TILT]+=((head_tracking_tilt.kp*tilt_error)>>16);
-      manager_current_angles[L_TILT]+=((head_tracking_tilt.ki*head_tracking_tilt.integral_part)>>16);
-      manager_current_angles[L_TILT]+=((head_tracking_tilt.kd*derivative_tilt)>>16);
-      if(manager_current_angles[L_TILT]>head_tracking_tilt.max_angle)
-        manager_current_angles[L_TILT]=head_tracking_tilt.max_angle;
-      else if(manager_current_angles[L_TILT]<head_tracking_tilt.min_angle)
-        manager_current_angles[L_TILT]=head_tracking_tilt.min_angle;
-      head_tracking_tilt.prev_error = tilt_error;
-    }
-  }
-  else
-    ram_data[DARWIN_HEAD_CNTRL]&=(~HEAD_STATUS);
-}
-
diff --git a/src/joint_motion.c b/src/joint_motion.c
deleted file mode 100644
index bd537d08b82d5557a02bbcbcc8535aae354557ba..0000000000000000000000000000000000000000
--- a/src/joint_motion.c
+++ /dev/null
@@ -1,285 +0,0 @@
-#include "joint_motion.h"
-#include "ram.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_target_stop_angles[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
-
-int64_t joint_motion_period;
-int16_t joint_motion_period_us;
-
-uint8_t joint_stop[NUM_JOINT_GROUPS];
-uint8_t joint_moving[NUM_JOINT_GROUPS];
-uint32_t joint_group_servos[NUM_JOINT_GROUPS];
-
-// public functions
-void joint_motion_init(uint16_t period_us)
-{
-  uint8_t i;
-
-  /* initialize the internal variables */
-  for(i=0;i<MANAGER_MAX_NUM_SERVOS;i++)
-  {
-    // target values
-    joint_target_angles[i]=manager_current_angles[i];
-    joint_target_speeds[i]=0;
-    joint_target_accels[i]=0;
-    // current values
-    joint_current_angles[i]=manager_current_angles[i];
-    joint_current_speeds[i]=0;
-    // the current angles, speeds and accelerations are in RAM
-    joint_dir[i]=0;
-  }
-  for(i=0;i<NUM_JOINT_GROUPS;i++)
-  {
-    joint_stop[i]=0x00;
-    joint_moving[i]=0x00;
-    joint_group_servos[i]=0x00000000;
-  }
-  joint_motion_period=(period_us<<16)/1000000;
-  joint_motion_period_us=period_us;
-}
-
-void joint_motion_set_period(uint16_t period_us)
-{
-  joint_motion_period=(period_us<<16)/1000000;
-  joint_motion_period_us=period_us;
-}
-
-uint16_t joint_motion_get_period(void)
-{
-  return joint_motion_period_us;
-}
-
-void joint_motion_set_group_servos(uint8_t group_id,uint32_t servos)
-{
-  joint_group_servos[group_id]=servos;
-  ram_data[DARWIN_JOINT_GRP0_SERVOS0+group_id*5]=servos&0x000000FF;
-  ram_data[DARWIN_JOINT_GRP0_SERVOS1+group_id*5]=(servos>>8)&0x000000FF;
-  ram_data[DARWIN_JOINT_GRP0_SERVOS2+group_id*5]=(servos>>16)&0x000000FF;
-  ram_data[DARWIN_JOINT_GRP0_SERVOS3+group_id*5]=(servos>>24)&0x000000FF;
-}
-
-uint32_t joint_motion_get_group_servos(uint8_t group_id)
-{
-  return joint_group_servos[group_id];
-}
-
-void joint_motion_start(uint8_t group_id)
-{
-  uint8_t i;
-  int16_t angle;
-  uint32_t servo_index;
-
-  /* initialize the internal variables */
-  for(i=0,servo_index=0x00000001;i<MANAGER_MAX_NUM_SERVOS;i++,servo_index=servo_index<<1)
-  {
-    if(joint_group_servos[group_id]&servo_index)
-    {
-      if(ram_data[DARWIN_JOINT_SERVO0_SPEED+i]==0)
-      {
-        joint_target_angles[i]=manager_current_angles[i];
-        joint_current_angles[i]=manager_current_angles[i];
-        angle=manager_current_angles[i]>>9;
-        ram_data[DARWIN_JOINT_SERVO0_ANGLE_L+i*2]=angle%256;
-        ram_data[DARWIN_JOINT_SERVO0_ANGLE_H+i*2]=angle/256;
-      }
-      else
-      {
-        // current values
-        joint_current_angles[i]=manager_current_angles[i];
-        // target angle
-        angle=ram_data[DARWIN_JOINT_SERVO0_ANGLE_L+i*2]+(ram_data[DARWIN_JOINT_SERVO0_ANGLE_H+i*2]<<8);
-        if(angle>manager_get_ccw_angle_limit(i))
-          angle=manager_get_ccw_angle_limit(i);
-        else if(angle<manager_get_cw_angle_limit(i))
-          angle=manager_get_cw_angle_limit(i);
-        joint_target_angles[i]=angle<<9;
-        // target speed
-        joint_target_speeds[i]=ram_data[DARWIN_JOINT_SERVO0_SPEED+i]<<16;
-        // target speed
-        joint_target_accels[i]=ram_data[DARWIN_JOINT_SERVO0_ACCEL+i]<<16;
-        if(joint_target_accels[i]==0)
-        {
-          joint_target_accels[i]=1<<16;
-          ram_data[DARWIN_JOINT_SERVO0_ACCEL+i]=1;
-        }
-        // check the parameters
-        if(abs(joint_target_angles[i]-joint_current_angles[i])>=65)
-        {
-          if((joint_target_speeds[i]*joint_target_speeds[i])/joint_target_accels[i]>abs(joint_target_angles[i]-joint_current_angles[i]))
-          {
-            joint_target_accels[i]=(joint_target_speeds[i]*joint_target_speeds[i])/abs(joint_target_angles[i]-joint_current_angles[i]);
-            ram_data[DARWIN_JOINT_SERVO0_ACCEL+i]=joint_target_accels[i]>>16;
-          }
-        }
-        // stop angles
-        joint_target_stop_angles[i]=joint_target_speeds[i]*joint_target_speeds[i]/(2*joint_target_accels[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[group_id]=0x01;
-  joint_stop[group_id]=0x00;
-  ram_data[DARWIN_JOINT_GRP0_CNTRL+group_id*5]|=JOINT_STATUS;
-}
-
-void joint_motion_stop(uint8_t group_id)
-{
-  joint_stop[group_id]=0x01;
-}
-
-uint8_t are_joints_moving(uint8_t group_id)
-{
-  return joint_moving[group_id];
-}
-
-// operation functions
-uint8_t joint_motion_in_range(unsigned short int address, unsigned short int length)
-{
-  if(ram_in_window(JOINT_BASE_ADDRESS,JOINT_MEM_LENGTH,address,length))
-    return 0x01;
-  else
-    return 0x00;
-}
-
-void joint_motion_process_read_cmd(unsigned short int address,unsigned short int length,unsigned char *data)
-{
-
-}
-
-void joint_motion_process_write_cmd(unsigned short int address,unsigned short int length,unsigned char *data)
-{
-  uint8_t j,k;
-  uint16_t i;
-  uint32_t servos;
-
-  // load motion information
-  for(i=DARWIN_JOINT_SERVO0_ANGLE_L;i<=DARWIN_JOINT_SERVO31_ACCEL;i++)
-    if(ram_in_range(i,address,length))
-      ram_data[i]=data[i-address];
-  // group servos
-  for(j=0;j<4;j++)
-  {
-    if(ram_in_window(DARWIN_JOINT_GRP0_SERVOS0+j*5,4,address,length))
-    {
-      servos=joint_motion_get_group_servos(j);
-      for(i=DARWIN_JOINT_GRP0_SERVOS0+j*5,k=0;i<=DARWIN_JOINT_GRP0_SERVOS3+j*5;i++,k++)
-        if(ram_in_range(i,address,length))
-          servos|=(data[i-address]<<(k*8));
-      joint_motion_set_group_servos(j,servos);
-    }
-    if(ram_in_range(DARWIN_JOINT_GRP0_CNTRL+j*5,address,length))
-    {
-      if(data[DARWIN_JOINT_GRP0_CNTRL+j*5-address]&JOINT_START)
-        joint_motion_start(j);
-      if(data[DARWIN_JOINT_GRP0_CNTRL+j*5-address]&JOINT_STOP)
-        joint_motion_stop(j);
-    }
-  }
-}
-
-// motion manager interface functions
-void joint_motion_process(void)
-{
-  uint8_t moving,i,j;
-  uint32_t servo_index;
-
-  for(j=0;j<4;j++)
-  {
-    moving=0x00;
-    if(joint_moving[j]==0x01)
-    {
-      for(i=0,servo_index=0x00000001;i<MANAGER_MAX_NUM_SERVOS;i++,servo_index=servo_index<<1)
-      {
-        if(manager_get_module(i)==MM_JOINTS && joint_group_servos[j]&servo_index)
-        {
-          if(joint_stop[j]==0x01)
-          {
-            joint_target_angles[i]=manager_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])>=65)
-            {
-              moving=0x01;
-              if(joint_current_speeds[i]!=joint_dir[i]*joint_target_speeds[i])// it is necessary to change the current speed
-              {
-                joint_current_angles[i]+=((joint_current_speeds[i]*joint_motion_period)>>17);
-                if(joint_current_speeds[i]>joint_dir[i]*joint_target_speeds[i])
-                {
-                  joint_current_speeds[i]-=((joint_target_accels[i]*joint_motion_period)>>16);// reduce speed
-                  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_target_accels[i]*joint_motion_period)>>16);// increase speed
-                  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_speeds[i]*joint_motion_period)>>17);
-                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_stop_angles[i])// constant speed phase
-                {
-                  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// deceleration phase
-                {
-                  joint_current_angles[i]=joint_current_angles[i]+((joint_current_speeds[i]*joint_motion_period)>>17);
-                  joint_target_speeds[i]=joint_target_speeds[i]-((joint_target_accels[i]*joint_motion_period)>>16);
-                  if(joint_target_speeds[i]<0)
-                    joint_target_speeds[i]=0;
-                  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)>>17);
-                  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_current_speeds[i]=0;
-            }
-            manager_current_angles[i]=joint_current_angles[i];
-            manager_current_slopes[i]=5;
-          }
-        }
-      }
-      if(moving==0)
-      {
-        joint_moving[j]=0x00;
-        joint_stop[j]=0x00;
-        ram_data[DARWIN_JOINT_GRP0_CNTRL+j*5]&=(~JOINT_STATUS);
-      }
-    }
-  }
-}
-
diff --git a/src/stairs.c b/src/stairs.c
deleted file mode 100755
index 42555871998c5a3e25655b333e53ae049db7e11f..0000000000000000000000000000000000000000
--- a/src/stairs.c
+++ /dev/null
@@ -1,580 +0,0 @@
-#include "stairs.h"
-#include "darwin_kinematics.h"
-#include "darwin_math.h"
-#include "ram.h"
-#include <math.h>
-
-typedef enum {SHIFT_WEIGHT_LEFT=0x10,RISE_RIGHT_FOOT=0x20,ADVANCE_RIGHT_FOOT=0x30,CONTACT_RIGHT_FOOT=0x40,SHIFT_WEIGHT_RIGHT=0x50,RISE_LEFT_FOOT=0x60,ADVANCE_LEFT_FOOT=0x70,CONTACT_LEFT_FOOT=0x80,CENTER_WEIGHT=0x90} stairs_phase_t;
-
-// internal motion variables
-float stairs_Z_stair_height;
-float stairs_Z_overshoot;
-float stairs_Y_shift_amplitude;
-float stairs_X_shift_amplitude;
-float stairs_R_shift_amplitude;
-float stairs_P_shift_amplitude;
-float stairs_A_shift_amplitude;
-float stairs_Y_spread_amplitude;
-float stairs_X_shift_body;
-
-// internal offset variables
-float stairs_Hip_Pitch_Offset;
-float stairs_X_Offset;
-float stairs_Y_Offset;
-float stairs_Z_Offset;
-float stairs_R_Offset;
-float stairs_P_Offset;
-float stairs_A_Offset;
-
-// internal time variables
-float stairs_Time;
-float stairs_period;
-float stairs_shift_weight_left_time;
-float stairs_rise_right_foot_time;
-float stairs_advance_right_foot_time;
-float stairs_contact_right_foot_time;
-float stairs_shift_weight_right_time;
-float stairs_rise_left_foot_time;
-float stairs_advance_left_foot_time;
-float stairs_contact_left_foot_time;
-float stairs_center_weight_time;
-
-// control variables
-uint8_t stairs_Ctrl_Running;
-uint8_t stairs_up;
-
-// private functions
-
-// public functions
-void stairs_init(uint16_t period_us)
-{
-  // initialize the internal motion variables
-
-
-  // initialize internal control variables
-  stairs_Time=0;
-  stairs_period=((float)period_us)/1000.0;
-  stairs_Ctrl_Running=0x00;
-  stairs_up=0x00;
-}
-
-void stairs_set_period(uint16_t period_us)
-{
-  stairs_period=((float)period_us)/1000.0;
-}
-
-uint16_t stairs_get_period(void)
-{
-  return (uint16_t)(stairs_period*1000);
-}
-
-void stairs_start(uint8_t up)
-{
-  // load all parameters
-  stairs_shift_weight_left_time=((float)ram_data[DARWIN_STAIRS_PHASE1_TIME_L]+(ram_data[DARWIN_STAIRS_PHASE1_TIME_H]<<8));
-  stairs_rise_right_foot_time=((float)ram_data[DARWIN_STAIRS_PHASE2_TIME_L]+(ram_data[DARWIN_STAIRS_PHASE2_TIME_H]<<8));
-  stairs_advance_right_foot_time=((float)ram_data[DARWIN_STAIRS_PHASE3_TIME_L]+(ram_data[DARWIN_STAIRS_PHASE3_TIME_H]<<8));
-  stairs_contact_right_foot_time=((float)ram_data[DARWIN_STAIRS_PHASE4_TIME_L]+(ram_data[DARWIN_STAIRS_PHASE4_TIME_H]<<8));
-  stairs_shift_weight_right_time=((float)ram_data[DARWIN_STAIRS_PHASE5_TIME_L]+(ram_data[DARWIN_STAIRS_PHASE5_TIME_H]<<8));
-  stairs_rise_left_foot_time=((float)ram_data[DARWIN_STAIRS_PHASE6_TIME_L]+(ram_data[DARWIN_STAIRS_PHASE6_TIME_H]<<8));
-  stairs_advance_left_foot_time=((float)ram_data[DARWIN_STAIRS_PHASE7_TIME_L]+(ram_data[DARWIN_STAIRS_PHASE7_TIME_H]<<8));
-  stairs_contact_left_foot_time=((float)ram_data[DARWIN_STAIRS_PHASE8_TIME_L]+(ram_data[DARWIN_STAIRS_PHASE8_TIME_H]<<8));
-  stairs_center_weight_time=((float)ram_data[DARWIN_STAIRS_PHASE9_TIME_L]+(ram_data[DARWIN_STAIRS_PHASE9_TIME_H]<<8));
-  stairs_Z_stair_height=((float)((int8_t)ram_data[DARWIN_STAIRS_Z_HEIGHT]));
-  stairs_Z_overshoot=((float)((int8_t)ram_data[DARWIN_STAIRS_Z_OVERSHOOT]));
-  stairs_Y_shift_amplitude=((float)((int8_t)ram_data[DARWIN_STAIRS_Y_SHIFT]));
-  stairs_X_shift_amplitude=((float)((int8_t)ram_data[DARWIN_STAIRS_X_SHIFT]));
-  stairs_Hip_Pitch_Offset=((float)((int16_t)(ram_data[DARWIN_WALK_HIP_PITCH_OFF_L]+(ram_data[DARWIN_WALK_HIP_PITCH_OFF_H]<<8))))/1024.0;
-  stairs_R_shift_amplitude=((float)((int8_t)ram_data[DARWIN_STAIRS_R_SHIFT]))*PI/720.0;// (r_shift/4)*(pi/180)
-  stairs_P_shift_amplitude=((float)((int8_t)ram_data[DARWIN_STAIRS_P_SHIFT]))*PI/720.0;// (p_shift/4)*(pi/180)
-  stairs_A_shift_amplitude=((float)((int8_t)ram_data[DARWIN_STAIRS_A_SHIFT]))*PI/720.0;// (a_shift/4)*(pi/180)
-  stairs_Y_spread_amplitude=((float)((int8_t)ram_data[DARWIN_STAIRS_Y_SPREAD]));
-  stairs_X_shift_body=((float)((int8_t)ram_data[DARWIN_STAIRS_X_SHIFT_BODY]));
-  stairs_X_Offset=((float)((int8_t)ram_data[DARWIN_STAIRS_X_OFFSET]));
-  stairs_Y_Offset=((float)((int8_t)ram_data[DARWIN_STAIRS_Y_OFFSET]));
-  stairs_Z_Offset=((float)((int8_t)ram_data[DARWIN_STAIRS_Z_OFFSET]));
-  stairs_R_Offset=((float)((int8_t)ram_data[DARWIN_STAIRS_X_OFFSET]))*PI/1440.0;
-  stairs_P_Offset=((float)((int8_t)ram_data[DARWIN_STAIRS_Y_OFFSET]))*PI/1440.0;
-  stairs_A_Offset=((float)((int8_t)ram_data[DARWIN_STAIRS_Z_OFFSET]))*PI/1440.0;
-  // start operation
-  ram_data[DARWIN_STAIRS_CNTRL]|=STAIRS_STATUS;
-  stairs_Ctrl_Running=0x01;
-  stairs_up=up;
-}
-
-void stairs_stop(void)
-{
-  stairs_Ctrl_Running=0x00;
-}
-
-uint8_t is_climbing_stairs(void)
-{
-  if(ram_data[DARWIN_STAIRS_CNTRL]&STAIRS_STATUS)
-    return 0x01;
-  else 
-    return 0x00;
-}
-
-uint8_t stairs_get_phase(void)
-{
-  return (int8_t)ram_data[DARWIN_STAIRS_CNTRL]&STAIRS_PHASE;
-}
-
-// motion manager interface functions
-void stairs_process(void)
-{
-  float angle[14]={0},ep[12]={0};
-  float delta;
-
-  if(ram_data[DARWIN_STAIRS_CNTRL]&STAIRS_STATUS)
-  {
-    ram_data[DARWIN_STAIRS_CNTRL]&=(~STAIRS_PHASE);
-    if(stairs_up)
-    {
-      if(stairs_Time>=0 && stairs_Time<=stairs_shift_weight_left_time)
-      {
-        //1
-        delta=stairs_Time/stairs_shift_weight_left_time;
-        ep[0]=stairs_X_Offset;
-        ep[1]=-stairs_Y_Offset/2.0+stairs_Y_shift_amplitude*delta;
-        ep[2]=stairs_Z_Offset;
-        ep[3]=stairs_R_Offset;
-        ep[4]=stairs_P_Offset;
-        ep[5]=stairs_A_Offset;
-        ep[6]=stairs_X_Offset;
-        ep[7]=stairs_Y_Offset/2.0+stairs_Y_shift_amplitude*delta;
-        ep[8]=stairs_Z_Offset;
-        ep[9]=stairs_R_Offset;
-        ep[10]=stairs_P_Offset;
-        ep[11]=stairs_A_Offset;
-        ram_data[DARWIN_STAIRS_CNTRL]|=SHIFT_WEIGHT_LEFT;
-      }
-      else if(stairs_Time>stairs_shift_weight_left_time && stairs_Time<=stairs_rise_right_foot_time)
-      {
-        //2
-        delta=(stairs_Time-stairs_shift_weight_left_time)/(stairs_rise_right_foot_time-stairs_shift_weight_left_time);
-        ep[0]=stairs_X_Offset;
-        ep[1]=-stairs_Y_Offset/2.0+stairs_Y_shift_amplitude;
-        ep[2]=stairs_Z_Offset;
-        ep[3]=stairs_R_Offset-stairs_R_shift_amplitude*delta;
-        ep[4]=stairs_P_Offset;
-        ep[5]=stairs_A_Offset;
-        ep[6]=stairs_X_Offset;
-        ep[7]=stairs_Y_Offset/2.0+stairs_Y_shift_amplitude;
-        ep[8]=stairs_Z_Offset+(stairs_Z_overshoot+stairs_Z_stair_height)*delta;
-        ep[9]=stairs_R_Offset-stairs_R_shift_amplitude*delta;
-        ep[10]=stairs_P_Offset;
-        ep[11]=stairs_A_Offset;
-        ram_data[DARWIN_STAIRS_CNTRL]|=RISE_RIGHT_FOOT;
-      }
-      else if(stairs_Time>stairs_rise_right_foot_time && stairs_Time<=stairs_advance_right_foot_time)
-      {   
-        //3
-        delta=(stairs_Time-stairs_rise_right_foot_time)/(stairs_advance_right_foot_time-stairs_rise_right_foot_time);
-        ep[0]=stairs_X_Offset;
-        ep[1]=-stairs_Y_Offset/2.0+stairs_Y_shift_amplitude-(stairs_Y_spread_amplitude/2.0)*delta;
-        ep[2]=stairs_Z_Offset;
-        ep[3]=stairs_R_Offset-stairs_R_shift_amplitude;
-        ep[4]=stairs_P_Offset+stairs_P_shift_amplitude*delta;
-        ep[5]=stairs_A_Offset+stairs_A_shift_amplitude*delta;
-        ep[6]=stairs_X_Offset+stairs_X_shift_amplitude*delta;
-        ep[7]=stairs_Y_Offset/2.0+stairs_Y_shift_amplitude+(stairs_Y_spread_amplitude/2.0)*delta;
-        ep[8]=stairs_Z_Offset+stairs_Z_overshoot+stairs_Z_stair_height;
-        ep[9]=stairs_R_Offset-stairs_R_shift_amplitude;
-        ep[10]=stairs_P_Offset+stairs_P_shift_amplitude*delta;
-        ep[11]=stairs_A_Offset+stairs_A_shift_amplitude*delta;
-        ram_data[DARWIN_STAIRS_CNTRL]|=ADVANCE_RIGHT_FOOT;
-      }
-      else if(stairs_Time>stairs_advance_right_foot_time && stairs_Time<=stairs_contact_right_foot_time)
-      {
-        //4
-        delta=(stairs_Time-stairs_advance_right_foot_time)/(stairs_contact_right_foot_time-stairs_advance_right_foot_time);
-        ep[0]=stairs_X_Offset;
-        ep[1]=-stairs_Y_Offset/2.0+stairs_Y_shift_amplitude-(stairs_Y_spread_amplitude/2.0);
-        ep[2]=stairs_Z_Offset;
-        ep[3]=stairs_R_Offset-stairs_R_shift_amplitude;
-        ep[4]=stairs_P_Offset+stairs_P_shift_amplitude;
-        ep[5]=stairs_A_Offset+stairs_A_shift_amplitude;
-        ep[6]=stairs_X_Offset+stairs_X_shift_amplitude;
-        ep[7]=stairs_Y_Offset/2.0+stairs_Y_shift_amplitude+(stairs_Y_spread_amplitude/2.0);
-        ep[8]=stairs_Z_Offset+stairs_Z_overshoot+stairs_Z_stair_height-stairs_Z_overshoot*delta;
-        ep[9]=stairs_R_Offset-stairs_R_shift_amplitude;
-        ep[10]=stairs_P_Offset+stairs_P_shift_amplitude;
-        ep[11]=stairs_A_Offset+stairs_A_shift_amplitude;
-        ram_data[DARWIN_STAIRS_CNTRL]|=CONTACT_RIGHT_FOOT;
-      }
-      else if(stairs_Time>stairs_contact_right_foot_time && stairs_Time<=stairs_shift_weight_right_time)
-      {
-        //5
-        delta=(stairs_Time-stairs_contact_right_foot_time)/(stairs_shift_weight_right_time-stairs_contact_right_foot_time);
-        ep[0]=stairs_X_Offset-stairs_X_shift_body*delta;
-        ep[1]=-stairs_Y_Offset/2.0+stairs_Y_shift_amplitude-(stairs_Y_spread_amplitude/2.0)-(2*stairs_Y_shift_amplitude)*delta;
-        ep[2]=stairs_Z_Offset;
-        ep[3]=stairs_R_Offset-stairs_R_shift_amplitude+2.0*stairs_R_shift_amplitude*delta;
-        ep[4]=stairs_P_Offset+stairs_P_shift_amplitude-3.0*stairs_P_shift_amplitude*delta;
-        ep[5]=stairs_A_Offset+stairs_A_shift_amplitude;
-        ep[6]=stairs_X_Offset+stairs_X_shift_amplitude-stairs_X_shift_body*delta;
-        ep[7]=stairs_Y_Offset/2.0+stairs_Y_shift_amplitude+(stairs_Y_spread_amplitude/2.0)-(2*stairs_Y_shift_amplitude)*delta;
-        ep[8]=stairs_Z_Offset+stairs_Z_stair_height;
-        ep[9]=stairs_R_Offset-stairs_R_shift_amplitude+2.0*stairs_R_shift_amplitude*delta;
-        ep[10]=stairs_P_Offset+stairs_P_shift_amplitude-3.0*stairs_P_shift_amplitude*delta;
-        ep[11]=stairs_A_Offset+stairs_A_shift_amplitude;
-        ram_data[DARWIN_STAIRS_CNTRL]|=SHIFT_WEIGHT_RIGHT;
-      }
-      else if(stairs_Time>stairs_shift_weight_right_time && stairs_Time<=stairs_rise_left_foot_time)
-      {
-        //6
-        delta=(stairs_Time-stairs_shift_weight_right_time)/(stairs_rise_left_foot_time-stairs_shift_weight_right_time);
-        ep[0]=stairs_X_Offset-stairs_X_shift_body;
-        ep[1]=-stairs_Y_Offset/2.0-stairs_Y_shift_amplitude-(stairs_Y_spread_amplitude/2.0);
-        ep[2]=stairs_Z_Offset+(stairs_Z_overshoot+stairs_Z_stair_height)*delta;
-        ep[3]=stairs_R_Offset+stairs_R_shift_amplitude;
-        ep[4]=stairs_P_Offset-2.0*stairs_P_shift_amplitude;
-        ep[5]=stairs_A_Offset+stairs_A_shift_amplitude;
-        ep[6]=stairs_X_Offset+stairs_X_shift_amplitude-stairs_X_shift_body;
-        ep[7]=stairs_Y_Offset/2.0-stairs_Y_shift_amplitude+(stairs_Y_spread_amplitude/2.0);
-        ep[8]=stairs_Z_Offset+stairs_Z_stair_height;
-        ep[9]=stairs_R_Offset+stairs_R_shift_amplitude;
-        ep[10]=stairs_P_Offset-2.0*stairs_P_shift_amplitude;
-        ep[11]=stairs_A_Offset+stairs_A_shift_amplitude;
-        ram_data[DARWIN_STAIRS_CNTRL]|=RISE_LEFT_FOOT;
-      }
-      else if(stairs_Time>stairs_rise_left_foot_time && stairs_Time<=stairs_advance_left_foot_time)
-      {
-        //7
-        delta=(stairs_Time-stairs_rise_left_foot_time)/(stairs_advance_left_foot_time-stairs_rise_left_foot_time);
-        ep[0]=stairs_X_Offset-stairs_X_shift_body+stairs_X_shift_body*delta;
-        ep[1]=-stairs_Y_Offset/2.0-stairs_Y_shift_amplitude-(stairs_Y_spread_amplitude/2.0)+(stairs_Y_spread_amplitude/2.0)*delta;
-        ep[2]=stairs_Z_Offset+stairs_Z_overshoot+stairs_Z_stair_height;
-        ep[3]=stairs_R_Offset+stairs_R_shift_amplitude;
-        ep[4]=stairs_P_Offset-2.0*stairs_P_shift_amplitude+stairs_P_shift_amplitude*delta;
-        ep[5]=stairs_A_Offset+stairs_A_shift_amplitude-stairs_A_shift_amplitude*delta;
-        ep[6]=stairs_X_Offset+stairs_X_shift_amplitude-stairs_X_shift_body-(stairs_X_shift_amplitude-stairs_X_shift_body)*delta;
-        ep[7]=stairs_Y_Offset/2.0-stairs_Y_shift_amplitude+(stairs_Y_spread_amplitude/2.0)-(stairs_Y_spread_amplitude/2.0)*delta;
-        ep[8]=stairs_Z_Offset+stairs_Z_stair_height;
-        ep[9]=stairs_R_Offset+stairs_R_shift_amplitude;
-        ep[10]=stairs_P_Offset-2.0*stairs_P_shift_amplitude+stairs_P_shift_amplitude*delta;
-        ep[11]=stairs_A_Offset+stairs_A_shift_amplitude-stairs_A_shift_amplitude*delta;
-        ram_data[DARWIN_STAIRS_CNTRL]|=ADVANCE_LEFT_FOOT;
-      }
-      else if(stairs_Time>stairs_advance_left_foot_time && stairs_Time<=stairs_contact_left_foot_time)
-      {
-        //8
-        delta=(stairs_Time-stairs_advance_left_foot_time)/(stairs_contact_left_foot_time-stairs_advance_left_foot_time);
-        ep[0]=stairs_X_Offset;
-        ep[1]=-stairs_Y_Offset/2.0-stairs_Y_shift_amplitude;
-        ep[2]=stairs_Z_Offset+stairs_Z_overshoot+stairs_Z_stair_height-stairs_Z_overshoot*delta;
-        ep[3]=stairs_R_Offset+stairs_R_shift_amplitude-stairs_R_shift_amplitude*delta;
-        ep[4]=stairs_P_Offset-stairs_P_shift_amplitude+stairs_P_shift_amplitude*delta;
-        ep[5]=stairs_A_Offset;
-        ep[6]=stairs_X_Offset;
-        ep[7]=stairs_Y_Offset/2.0-stairs_Y_shift_amplitude;
-        ep[8]=stairs_Z_Offset+stairs_Z_stair_height;
-        ep[9]=stairs_R_Offset+stairs_R_shift_amplitude-stairs_R_shift_amplitude*delta;
-        ep[10]=stairs_P_Offset-stairs_P_shift_amplitude+stairs_P_shift_amplitude*delta;
-        ep[11]=stairs_A_Offset;
-        ram_data[DARWIN_STAIRS_CNTRL]|=CONTACT_LEFT_FOOT;
-      }
-      else if(stairs_Time>stairs_contact_left_foot_time && stairs_Time<=stairs_center_weight_time)
-      {
-        //9
-        delta=(stairs_Time-stairs_contact_left_foot_time)/(stairs_center_weight_time-stairs_contact_left_foot_time);
-        ep[0]=stairs_X_Offset;
-        ep[1]=-stairs_Y_Offset/2.0-stairs_Y_shift_amplitude+stairs_Y_shift_amplitude*delta;
-        ep[2]=stairs_Z_Offset+stairs_Z_stair_height-stairs_Z_stair_height*delta;
-        ep[3]=stairs_R_Offset;
-        ep[4]=stairs_P_Offset;
-        ep[5]=stairs_A_Offset;
-        ep[6]=stairs_X_Offset;
-        ep[7]=stairs_Y_Offset/2.0-stairs_Y_shift_amplitude+stairs_Y_shift_amplitude*delta;
-        ep[8]=stairs_Z_Offset+stairs_Z_stair_height-stairs_Z_stair_height*delta;
-        ep[9]=stairs_R_Offset;
-        ep[10]=stairs_P_Offset;
-        ep[11]=stairs_A_Offset;
-        ram_data[DARWIN_STAIRS_CNTRL]|=CENTER_WEIGHT;
-      }
-      else
-      {
-        ram_data[DARWIN_STAIRS_CNTRL]&=(~STAIRS_STATUS);
-        stairs_Ctrl_Running=0x00;
-      }
-    }
-    else
-    {   
-      if(stairs_Time>=0 && stairs_Time<=stairs_shift_weight_left_time)
-      {
-        //1
-        delta=stairs_Time/stairs_shift_weight_left_time;
-        ep[0]=stairs_X_Offset;
-        ep[1]=stairs_Y_Offset/2.0+stairs_Y_shift_amplitude*delta;
-        ep[2]=stairs_Z_Offset+stairs_Z_stair_height*delta;
-        ep[3]=stairs_R_Offset;
-        ep[4]=stairs_P_Offset;
-        ep[5]=stairs_A_Offset;
-        ep[6]=stairs_X_Offset;
-        ep[7]=stairs_Y_Offset/2.0+stairs_Y_shift_amplitude*delta;
-        ep[8]=stairs_Z_Offset+stairs_Z_stair_height*delta;
-        ep[9]=stairs_R_Offset;
-        ep[10]=stairs_P_Offset;
-        ep[11]=stairs_A_Offset;
-        ram_data[DARWIN_STAIRS_CNTRL]|=SHIFT_WEIGHT_LEFT;
-      }
-      else if(stairs_Time>stairs_shift_weight_left_time && stairs_Time<=stairs_rise_right_foot_time)
-      {
-        //2
-        delta=(stairs_Time-stairs_shift_weight_left_time)/(stairs_rise_right_foot_time-stairs_shift_weight_left_time);
-        ep[0]=stairs_X_Offset;
-        ep[1]=stairs_Y_Offset/2.0+stairs_Y_shift_amplitude;
-        ep[2]=stairs_Z_Offset+stairs_Z_stair_height;
-        ep[3]=stairs_R_Offset-stairs_R_shift_amplitude;
-        ep[4]=stairs_P_Offset;
-        ep[5]=stairs_A_Offset;
-        ep[6]=stairs_X_Offset;
-        ep[7]=stairs_Y_Offset/2.0+stairs_Y_shift_amplitude;
-        ep[8]=stairs_Z_Offset+stairs_Z_stair_height+stairs_Z_overshoot*delta;
-        ep[9]=stairs_R_Offset-stairs_R_shift_amplitude*delta;
-        ep[10]=stairs_P_Offset;
-        ep[11]=stairs_A_Offset;
-        ram_data[DARWIN_STAIRS_CNTRL]|=RISE_RIGHT_FOOT;
-      }
-      else if(stairs_Time>stairs_rise_right_foot_time && stairs_Time<=stairs_advance_right_foot_time)
-      {   
-        //3
-        delta=(stairs_Time-stairs_rise_right_foot_time)/(stairs_advance_right_foot_time-stairs_rise_right_foot_time);
-        ep[0]=stairs_X_Offset;
-        ep[1]=stairs_Y_Offset/2.0+stairs_Y_shift_amplitude-(stairs_Y_spread_amplitude/2.0)*delta;
-        ep[2]=stairs_Z_Offset+stairs_Z_stair_height;
-        ep[3]=stairs_R_Offset-stairs_R_shift_amplitude;
-        ep[4]=stairs_P_Offset+stairs_P_shift_amplitude*delta;
-        ep[5]=stairs_A_Offset+stairs_A_shift_amplitude*delta;
-        ep[6]=stairs_X_Offset+stairs_X_shift_amplitude*delta;
-        ep[7]=stairs_Y_Offset/2.0+stairs_Y_shift_amplitude+(stairs_Y_spread_amplitude/2.0)*delta;
-        ep[8]=stairs_Z_Offset+stairs_Z_overshoot+stairs_Z_stair_height;
-        ep[9]=stairs_R_Offset-stairs_R_shift_amplitude;
-        ep[10]=stairs_P_Offset+stairs_P_shift_amplitude*delta;
-        ep[11]=stairs_A_Offset+stairs_A_shift_amplitude*delta;
-        ram_data[DARWIN_STAIRS_CNTRL]|=ADVANCE_RIGHT_FOOT;
-      }
-      else if(stairs_Time>stairs_advance_right_foot_time && stairs_Time<=stairs_contact_right_foot_time)
-      {
-        //4
-        delta=(stairs_Time-stairs_advance_right_foot_time)/(stairs_contact_right_foot_time-stairs_advance_right_foot_time);
-        ep[0]=stairs_X_Offset;
-        ep[1]=stairs_Y_Offset/2.0+stairs_Y_shift_amplitude-(stairs_Y_spread_amplitude/2.0);
-        ep[2]=stairs_Z_Offset+stairs_Z_stair_height+stairs_Z_overshoot*delta;
-        ep[3]=stairs_R_Offset-stairs_R_shift_amplitude;
-        ep[4]=stairs_P_Offset+stairs_P_shift_amplitude;
-        ep[5]=stairs_A_Offset+stairs_A_shift_amplitude;
-        ep[6]=stairs_X_Offset+stairs_X_shift_amplitude;
-        ep[7]=stairs_Y_Offset/2.0+stairs_Y_shift_amplitude+(stairs_Y_spread_amplitude/2.0);
-        ep[8]=stairs_Z_Offset+stairs_Z_overshoot+stairs_Z_stair_height-stairs_Z_stair_height*delta;
-        ep[9]=stairs_R_Offset-stairs_R_shift_amplitude;
-        ep[10]=stairs_P_Offset+stairs_P_shift_amplitude;
-        ep[11]=stairs_A_Offset+stairs_A_shift_amplitude;
-        ram_data[DARWIN_STAIRS_CNTRL]|=CONTACT_RIGHT_FOOT;
-      }
-      else if(stairs_Time>stairs_contact_right_foot_time && stairs_Time<=stairs_shift_weight_right_time)
-      {
-        //5
-        delta=(stairs_Time-stairs_contact_right_foot_time)/(stairs_shift_weight_right_time-stairs_contact_right_foot_time);
-        ep[0]=stairs_X_Offset-stairs_X_shift_body*delta;
-        ep[1]=stairs_Y_Offset/2.0+stairs_Y_shift_amplitude-(stairs_Y_spread_amplitude/2.0)-(2*stairs_Y_shift_amplitude)*delta;
-        ep[2]=stairs_Z_Offset+stairs_Z_stair_height+stairs_Z_overshoot;
-        ep[3]=stairs_R_Offset-stairs_R_shift_amplitude+2.0*stairs_R_shift_amplitude*delta;
-        ep[4]=stairs_P_Offset+stairs_P_shift_amplitude-(2.0*stairs_P_shift_amplitude)*delta;
-        ep[5]=stairs_A_Offset+stairs_A_shift_amplitude;
-        ep[6]=stairs_X_Offset+stairs_X_shift_amplitude-stairs_X_shift_body*delta;
-        ep[7]=stairs_Y_Offset/2.0+stairs_Y_shift_amplitude+(stairs_Y_spread_amplitude/2.0)-(2*stairs_Y_shift_amplitude)*delta;
-        ep[8]=stairs_Z_Offset+stairs_Z_overshoot;
-        ep[9]=stairs_R_Offset-stairs_R_shift_amplitude+2.0*stairs_R_shift_amplitude*delta;
-        ep[10]=stairs_P_Offset+stairs_P_shift_amplitude-(2.0*stairs_P_shift_amplitude)*delta;
-        ep[11]=stairs_A_Offset+stairs_A_shift_amplitude;
-        ram_data[DARWIN_STAIRS_CNTRL]|=SHIFT_WEIGHT_RIGHT;
-      }
-      else if(stairs_Time>stairs_shift_weight_right_time && stairs_Time<=stairs_rise_left_foot_time)
-      {
-        //6
-        delta=(stairs_Time-stairs_shift_weight_right_time)/(stairs_rise_left_foot_time-stairs_shift_weight_right_time);
-        ep[0]=stairs_X_Offset-stairs_X_shift_body;
-        ep[1]=stairs_Y_Offset/2.0-stairs_Y_shift_amplitude-(stairs_Y_spread_amplitude/2.0);
-        ep[2]=stairs_Z_Offset+stairs_Z_overshoot+stairs_Z_stair_height;
-        ep[3]=stairs_R_Offset+stairs_R_shift_amplitude;
-        ep[4]=stairs_P_Offset-1.0*stairs_P_shift_amplitude;
-        ep[5]=stairs_A_Offset+stairs_A_shift_amplitude;
-        ep[6]=stairs_X_Offset+stairs_X_shift_amplitude-stairs_X_shift_body;
-        ep[7]=stairs_Y_Offset/2.0-stairs_Y_shift_amplitude+(stairs_Y_spread_amplitude/2.0);
-        ep[8]=stairs_Z_Offset+stairs_Z_overshoot-stairs_Z_overshoot*delta;
-        ep[9]=stairs_R_Offset+stairs_R_shift_amplitude;
-        ep[10]=stairs_P_Offset-1.0*stairs_P_shift_amplitude;
-        ep[11]=stairs_A_Offset+stairs_A_shift_amplitude;
-        ram_data[DARWIN_STAIRS_CNTRL]|=RISE_LEFT_FOOT;
-      }
-      else if(stairs_Time>stairs_rise_left_foot_time && stairs_Time<=stairs_advance_left_foot_time)
-      {
-        //7
-        delta=(stairs_Time-stairs_rise_left_foot_time)/(stairs_advance_left_foot_time-stairs_rise_left_foot_time);
-        ep[0]=stairs_X_Offset-stairs_X_shift_body+stairs_X_shift_body*delta;
-        ep[1]=stairs_Y_Offset/2.0-stairs_Y_shift_amplitude-(stairs_Y_spread_amplitude/2.0)+(stairs_Y_spread_amplitude/2.0)*delta;
-        ep[2]=stairs_Z_Offset+stairs_Z_overshoot+stairs_Z_stair_height;
-        ep[3]=stairs_R_Offset+stairs_R_shift_amplitude;//+stairs_R_shift_amplitude*delta;
-        ep[4]=stairs_P_Offset-1.0*stairs_P_shift_amplitude+1.0*stairs_P_shift_amplitude*delta;
-        ep[5]=stairs_A_Offset+stairs_A_shift_amplitude-stairs_A_shift_amplitude*delta;
-        ep[6]=stairs_X_Offset+stairs_X_shift_amplitude-stairs_X_shift_body-(stairs_X_shift_amplitude-stairs_X_shift_body)*delta;
-        ep[7]=stairs_Y_Offset/2.0-stairs_Y_shift_amplitude+(stairs_Y_spread_amplitude/2.0)-(stairs_Y_spread_amplitude/2.0)*delta;
-        ep[8]=stairs_Z_Offset;
-        ep[9]=stairs_R_Offset+stairs_R_shift_amplitude;//+stairs_R_shift_amplitude*delta;
-        ep[10]=stairs_P_Offset-1.0*stairs_P_shift_amplitude+1.0*stairs_P_shift_amplitude*delta;
-        ep[11]=stairs_A_Offset+stairs_A_shift_amplitude-stairs_A_shift_amplitude*delta;
-        ram_data[DARWIN_STAIRS_CNTRL]|=ADVANCE_LEFT_FOOT;
-      }
-      else if(stairs_Time>stairs_advance_left_foot_time && stairs_Time<=stairs_contact_left_foot_time)
-      {
-        //8
-        delta=(stairs_Time-stairs_advance_left_foot_time)/(stairs_contact_left_foot_time-stairs_advance_left_foot_time);
-        ep[0]=stairs_X_Offset;
-        ep[1]=stairs_Y_Offset/2.0-stairs_Y_shift_amplitude;
-        ep[2]=stairs_Z_Offset+stairs_Z_overshoot+stairs_Z_stair_height-(stairs_Z_stair_height+stairs_Z_overshoot)*delta;
-        ep[3]=stairs_R_Offset+stairs_R_shift_amplitude-stairs_R_shift_amplitude*delta;
-        ep[4]=stairs_P_Offset;
-        ep[5]=stairs_A_Offset;
-        ep[6]=stairs_X_Offset;
-        ep[7]=stairs_Y_Offset/2.0-stairs_Y_shift_amplitude;
-        ep[8]=stairs_Z_Offset;
-        ep[9]=stairs_R_Offset+stairs_R_shift_amplitude-stairs_R_shift_amplitude*delta;
-        ep[10]=stairs_P_Offset;
-        ep[11]=stairs_A_Offset;
-        ram_data[DARWIN_STAIRS_CNTRL]|=CONTACT_LEFT_FOOT;
-      }
-      else if(stairs_Time>stairs_contact_left_foot_time && stairs_Time<=stairs_center_weight_time)
-      {
-        //9
-        delta=(stairs_Time-stairs_contact_left_foot_time)/(stairs_center_weight_time-stairs_contact_left_foot_time);
-        ep[0]=stairs_X_Offset;
-        ep[1]=stairs_Y_Offset/2.0-stairs_Y_shift_amplitude+stairs_Y_shift_amplitude*delta;
-        ep[2]=stairs_Z_Offset;
-        ep[3]=stairs_R_Offset+stairs_R_shift_amplitude-stairs_R_shift_amplitude*delta;
-        ep[4]=stairs_P_Offset;
-        ep[5]=stairs_A_Offset;
-        ep[6]=stairs_X_Offset;
-        ep[7]=stairs_Y_Offset/2.0-stairs_Y_shift_amplitude+stairs_Y_shift_amplitude*delta;
-        ep[8]=stairs_Z_Offset;
-        ep[9]=stairs_R_Offset+stairs_R_shift_amplitude-stairs_R_shift_amplitude*delta;
-        ep[10]=stairs_P_Offset;
-        ep[11]=stairs_A_Offset;
-        ram_data[DARWIN_STAIRS_CNTRL]|=CENTER_WEIGHT;
-      }
-      else
-      {
-        ram_data[DARWIN_STAIRS_CNTRL]&=(~STAIRS_STATUS);
-        stairs_Ctrl_Running=0x00;
-      }
-    }
-    
-    if(ram_data[DARWIN_STAIRS_CNTRL]&STAIRS_STATUS)
-      stairs_Time += stairs_period;
-    else
-      stairs_Time=0;
-
-    // Compute angles with the inverse kinematics
-    if((darwin_leg_ik(&angle[0],ep[0],ep[1],ep[2],ep[3],ep[4],ep[5])!=1) || 
-       (darwin_leg_ik(&angle[6],ep[6],ep[7],ep[8],ep[9],ep[10],ep[11])!=1))
-      return;// Do not use angles
-
-    // Compute motor value
-    if(manager_get_module(R_HIP_YAW)==MM_WALKING)
-    {
-      manager_current_angles[R_HIP_YAW]=((-180.0*angle[0])/PI)*65536.0;
-      manager_current_slopes[R_HIP_YAW]=5;
-    }
-    if(manager_get_module(R_HIP_ROLL)==MM_WALKING)
-    {
-      manager_current_angles[R_HIP_ROLL]=((-180.0*angle[1])/PI)*65536.0;
-      manager_current_slopes[R_HIP_ROLL]=5;
-    }
-    if(manager_get_module(R_HIP_PITCH)==MM_WALKING)
-    {
-      manager_current_angles[R_HIP_PITCH]=((180.0*angle[2])/PI-stairs_Hip_Pitch_Offset)*65536.0;
-      manager_current_slopes[R_HIP_PITCH]=5;
-    }
-    if(manager_get_module(R_KNEE)==MM_WALKING)
-    {
-      manager_current_angles[R_KNEE]=((180.0*angle[3])/PI)*65536.0;
-      manager_current_slopes[R_KNEE]=5;
-    }
-    if(manager_get_module(R_ANKLE_PITCH)==MM_WALKING)
-    {
-      manager_current_angles[R_ANKLE_PITCH]=((-180.0*angle[4])/PI)*65536.0;
-      manager_current_slopes[R_ANKLE_PITCH]=5;
-    }
-    if(manager_get_module(R_ANKLE_ROLL)==MM_WALKING)
-    {
-      manager_current_angles[R_ANKLE_ROLL]=((180.0*angle[5])/PI)*65536.0;
-      manager_current_slopes[R_ANKLE_ROLL]=5;
-    }
-    if(manager_get_module(L_HIP_YAW)==MM_WALKING)
-    {
-      manager_current_angles[L_HIP_YAW]=((-180.0*angle[6])/PI)*65536.0;
-      manager_current_slopes[L_HIP_YAW]=5;
-    }
-    if(manager_get_module(L_HIP_ROLL)==MM_WALKING)
-    {
-      manager_current_angles[L_HIP_ROLL]=((-180.0*angle[7])/PI)*65536.0;
-      manager_current_slopes[L_HIP_ROLL]=5;
-    }
-    if(manager_get_module(L_HIP_PITCH)==MM_WALKING)
-    {
-      manager_current_angles[L_HIP_PITCH]=((-180.0*angle[8])/PI+stairs_Hip_Pitch_Offset)*65536.0;
-      manager_current_slopes[L_HIP_PITCH]=5;
-    }
-    if(manager_get_module(L_KNEE)==MM_WALKING)
-    {
-      manager_current_angles[L_KNEE]=((-180.0*angle[9])/PI)*65536.0;
-      manager_current_slopes[L_KNEE]=5;
-    }
-    if(manager_get_module(L_ANKLE_PITCH)==MM_WALKING)
-    {
-      manager_current_angles[L_ANKLE_PITCH]=((180.0*angle[10])/PI)*65536.0;
-      manager_current_slopes[L_ANKLE_PITCH]=5;
-    }
-    if(manager_get_module(L_ANKLE_ROLL)==MM_WALKING)
-    {
-      manager_current_angles[L_ANKLE_ROLL]=((180.0*angle[11])/PI)*65536.0;
-      manager_current_slopes[L_ANKLE_ROLL]=5;
-    }
-  }
-}
-
-// operation functions
-uint8_t stairs_in_range(unsigned short int address, unsigned short int length)
-{
-  if(ram_in_window(STAIRS_BASE_ADDRESS,STAIRS_MEM_LENGTH,address,length) ||
-     ram_in_window(STAIRS_EEPROM_ADDRESS,STAIRS_EEPROM_LENGTH,address,length))
-    return 0x01;
-  else
-    return 0x00;
-}
-
-void stairs_process_read_cmd(unsigned short int address,unsigned short int length,unsigned char *data)
-{
-
-}
-
-void stairs_process_write_cmd(unsigned short int address,unsigned short int length,unsigned char *data)
-{
-  uint16_t i;
-
-  // walk control
-  if(ram_in_range(DARWIN_STAIRS_CNTRL,address,length))
-  {
-    if(data[DARWIN_STAIRS_CNTRL-address]&STAIRS_START_UP)
-      stairs_start(0x01);
-    if(data[DARWIN_STAIRS_CNTRL-address]&STAIRS_START_DOWN)
-      stairs_start(0x00);
-    if(data[DARWIN_STAIRS_CNTRL-address]&STAIRS_STOP)
-      stairs_stop();
-  }
-  for(i=STAIRS_EEPROM_ADDRESS;i<=STAIRS_EEPROM_ADDRESS+STAIRS_EEPROM_LENGTH;i++)
-    if(ram_in_range(i,address,length))
-      ram_data[i]=data[i-address];
-}
-
diff --git a/src/walking.c b/src/walking.c
deleted file mode 100755
index 57277dec02844b3cb7f8eb400228833f35e35f30..0000000000000000000000000000000000000000
--- a/src/walking.c
+++ /dev/null
@@ -1,515 +0,0 @@
-#include "walking.h"
-#include "darwin_kinematics.h"
-#include "darwin_math.h"
-#include "ram.h"
-#include <math.h>
-
-enum {PHASE0=0x00,PHASE1=0x40,PHASE2=0x80,PHASE3=0xC0};
-
-// Walking control
-uint8_t A_MOVE_AIM_ON=0x00;
-
-// internal motion variables
-float m_X_Swap_Phase_Shift;
-float m_X_Swap_Amplitude;
-float m_X_Swap_Amplitude_Shift;
-float m_X_Move_Phase_Shift;
-float m_X_Move_Amplitude;
-float m_X_Move_Amplitude_Shift;
-float m_Y_Swap_Phase_Shift;
-float m_Y_Swap_Amplitude;
-float m_Y_Swap_Amplitude_Shift;
-float m_Y_Move_Phase_Shift;
-float m_Y_Move_Amplitude;
-float m_Y_Move_Amplitude_Shift;
-float m_Z_Swap_Phase_Shift;
-float m_Z_Swap_Amplitude;
-float m_Z_Swap_Amplitude_Shift;
-float m_Z_Move_Phase_Shift;
-float m_Z_Move_Amplitude;
-float m_Z_Move_Amplitude_Shift;
-float m_A_Move_Phase_Shift;
-float m_A_Move_Amplitude;
-float m_A_Move_Amplitude_Shift;
-
-// internal offset variables
-float m_Hip_Pitch_Offset;
-float m_Pelvis_Offset;
-float m_Pelvis_Swing;
-float m_Arm_Swing_Gain;
-float m_X_Offset;
-float m_Y_Offset;
-float m_Z_Offset;
-float m_R_Offset;
-float m_P_Offset;
-float m_A_Offset;
-
-// internal time variables
-float m_Time;
-float m_PeriodTime;
-float m_X_Swap_PeriodTime;
-float m_X_Move_PeriodTime;
-float m_Y_Swap_PeriodTime;
-float m_Y_Move_PeriodTime;
-float m_Z_Swap_PeriodTime;
-float m_Z_Move_PeriodTime;
-float m_A_Move_PeriodTime;
-float m_SSP_Time_Start_L;
-float m_SSP_Time_End_L;
-float m_SSP_Time_Start_R;
-float m_SSP_Time_End_R;
-float m_Phase_Time1;
-float m_Phase_Time2;
-float m_Phase_Time3;
-
-// control variables
-uint8_t m_Ctrl_Running;
-float walking_period;
-
-// private functions
-float wsin(float time, float period, float period_shift, float mag, float mag_shift)
-{
-  return mag*sin(((2.0*PI*time)/period)-period_shift)+mag_shift;
-}
-
-void update_param_time()
-{
-  float m_SSP_Ratio;
-  float m_SSP_Time;
-
-  m_PeriodTime=((float)((int16_t)(ram_data[DARWIN_WALK_PERIOD_TIME_L]+(ram_data[DARWIN_WALK_PERIOD_TIME_H]<<8))));
-  m_SSP_Ratio=1.0-((float)ram_data[DARWIN_WALK_DSP_RATIO])/256.0;
-
-  m_SSP_Time=m_PeriodTime*m_SSP_Ratio;
-
-  m_X_Swap_PeriodTime=m_PeriodTime/2.0;
-  m_X_Move_PeriodTime=m_SSP_Time;
-  m_Y_Swap_PeriodTime=m_PeriodTime;
-  m_Y_Move_PeriodTime=m_SSP_Time;
-  m_Z_Swap_PeriodTime=m_PeriodTime/2.0;
-  m_Z_Move_PeriodTime=m_SSP_Time/2.0;
-  m_A_Move_PeriodTime=m_SSP_Time;
-
-  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;
-  m_SSP_Time_End_R=(3+m_SSP_Ratio)*m_PeriodTime/4.0;
-
-  m_Phase_Time1=(m_SSP_Time_End_L+m_SSP_Time_Start_L)/2.0;
-  m_Phase_Time2=(m_SSP_Time_Start_R+m_SSP_Time_End_L)/2.0;
-  m_Phase_Time3=(m_SSP_Time_End_R+m_SSP_Time_Start_R)/2.0;
-
-  // m_pelvis_offset and m_pelvis_Swing in degrees
-  m_Pelvis_Offset=((float)ram_data[DARWIN_WALK_PELVIS_OFFSET])*PI/1440.0;
-  m_Pelvis_Swing=m_Pelvis_Offset*0.35;
-  m_Arm_Swing_Gain=((float)ram_data[DARWIN_WALK_ARM_SWING_GAIN])/32.0;
-}
-
-void update_param_move()
-{
-  float target_x_amplitude;
-  float target_y_amplitude;
-  float target_a_amplitude;
-  static float current_x_amplitude=0;
-  static float current_y_amplitude=0;
-  static float current_a_amplitude=0;
-
-  target_x_amplitude=((float)((int8_t)ram_data[DARWIN_WALK_STEP_FW_BW]));
-  // change longitudinal and transversal velocities to get to the target ones
-  if(current_x_amplitude<target_x_amplitude)
-  {
-    current_x_amplitude+=(((float)ram_data[DARWIN_WALK_MAX_VEL])*m_PeriodTime)/1000.0;
-    if(current_x_amplitude>target_x_amplitude)
-      current_x_amplitude=target_x_amplitude;
-  }
-  else if(current_x_amplitude>target_x_amplitude)
-  {
-    current_x_amplitude-=(((float)ram_data[DARWIN_WALK_MAX_VEL])*m_PeriodTime)/1000.0;
-    if(current_x_amplitude<target_x_amplitude)
-      current_x_amplitude=target_x_amplitude;
-  }
-  m_X_Move_Amplitude=current_x_amplitude;
-  m_X_Swap_Amplitude=current_x_amplitude*((float)ram_data[DARWIN_WALK_STEP_FW_BW_RATIO])/256.0;
-
-  // Right/Left
-  target_y_amplitude=((float)((int8_t)ram_data[DARWIN_WALK_STEP_LEFT_RIGHT]));
-  if(current_y_amplitude<target_y_amplitude)
-  {
-    current_y_amplitude+=(((float)ram_data[DARWIN_WALK_MAX_VEL])*m_PeriodTime)/1000.0;
-    if(current_y_amplitude>target_y_amplitude)
-      current_y_amplitude=target_y_amplitude;
-  }
-  else if(current_y_amplitude>target_y_amplitude)
-  {
-    current_y_amplitude-=(((float)ram_data[DARWIN_WALK_MAX_VEL])*m_PeriodTime)/1000.0;
-    if(current_y_amplitude<target_y_amplitude)
-      current_y_amplitude=target_y_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
-    m_Y_Move_Amplitude_Shift=-m_Y_Move_Amplitude;
-  m_Y_Swap_Amplitude=((float)ram_data[DARWIN_WALK_SWING_RIGHT_LEFT])+m_Y_Move_Amplitude_Shift*0.04;
-
-  m_Z_Move_Amplitude=((float)ram_data[DARWIN_WALK_FOOT_HEIGHT])/2.0;
-  m_Z_Move_Amplitude_Shift=m_Z_Move_Amplitude / 2.0;
-  m_Z_Swap_Amplitude=((float)ram_data[DARWIN_WALK_SWING_TOP_DOWN]);
-  m_Z_Swap_Amplitude_Shift=m_Z_Swap_Amplitude;
-
-  // Direction
-  target_a_amplitude=((float)((int8_t)ram_data[DARWIN_WALK_STEP_DIRECTION]))/8.0;
-  if(current_a_amplitude<target_a_amplitude)
-  {
-    current_a_amplitude+=(((float)ram_data[DARWIN_WALK_MAX_ROT_VEL])*m_PeriodTime)/8000.0;
-    if(current_a_amplitude>target_a_amplitude)
-      current_a_amplitude=target_a_amplitude;
-  }
-  else if(current_a_amplitude>target_a_amplitude)
-  {
-    current_a_amplitude-=(((float)ram_data[DARWIN_WALK_MAX_ROT_VEL])*m_PeriodTime)/8000.0;
-    if(current_a_amplitude<target_a_amplitude)
-      current_a_amplitude=target_a_amplitude;
-  }
-
-  if(A_MOVE_AIM_ON==0x00)
-  {
-    m_A_Move_Amplitude=current_a_amplitude*PI/180.0/2.0;
-    if(m_A_Move_Amplitude>0)
-      m_A_Move_Amplitude_Shift=m_A_Move_Amplitude;
-    else
-      m_A_Move_Amplitude_Shift=-m_A_Move_Amplitude;
-  }
-  else
-  {
-    m_A_Move_Amplitude=-current_a_amplitude*PI/180.0/2.0;
-    if(m_A_Move_Amplitude>0)
-      m_A_Move_Amplitude_Shift=-m_A_Move_Amplitude;
-    else
-      m_A_Move_Amplitude_Shift=m_A_Move_Amplitude;
-  }
-}
-
-void update_param_balance()
-{
-  m_X_Offset = ((float)((int8_t)ram_data[DARWIN_WALK_X_OFFSET]));
-  m_Y_Offset = ((float)((int8_t)ram_data[DARWIN_WALK_Y_OFFSET]));
-  m_Z_Offset = ((float)((int8_t)ram_data[DARWIN_WALK_Z_OFFSET]));
-  m_R_Offset = ((float)((int8_t)ram_data[DARWIN_WALK_ROLL_OFFSET]))*PI/1440.0;// (r_offset/8)*(pi/180)
-  m_P_Offset = ((float)((int8_t)ram_data[DARWIN_WALK_PITCH_OFFSET]))*PI/1440.0;// (p_offset/8)*(pi/180)
-  m_A_Offset = ((float)((int8_t)ram_data[DARWIN_WALK_YAW_OFFSET]))*PI/1440.0;// (a_offset/8)*(pi/180)
-  m_Hip_Pitch_Offset = ((float)((int16_t)(ram_data[DARWIN_WALK_HIP_PITCH_OFF_L]+(ram_data[DARWIN_WALK_HIP_PITCH_OFF_H]<<8))))/1024.0;
-}
-
-// public functions
-void walking_init(uint16_t period_us)
-{
-  // initialize the internal motion variables
-  m_X_Swap_Phase_Shift=PI;
-  m_X_Swap_Amplitude_Shift=0.0;
-  m_X_Move_Phase_Shift=PI/2.0;
-  m_X_Move_Amplitude=0.0;
-  m_X_Move_Amplitude_Shift=0.0;
-  m_Y_Swap_Phase_Shift=0.0;
-  m_Y_Swap_Amplitude_Shift=0.0;
-  m_Y_Move_Phase_Shift=PI/2.0;
-  m_Z_Swap_Phase_Shift=PI*3.0/2.0;
-  m_Z_Move_Phase_Shift=PI/2.0;
-  m_A_Move_Phase_Shift=PI/2.0;
-
-  // initialize internal control variables
-  m_Time=0;
-  walking_period=((float)period_us)/1000.0;
-  m_Ctrl_Running=0x00;
-
-  update_param_time();
-  update_param_move();
-
-  walking_process();
-}
-
-void walking_set_period(uint16_t period_us)
-{
-  walking_period=((float)period_us)/1000.0;
-}
-
-uint16_t walking_get_period(void)
-{
-  return (uint16_t)(walking_period*1000);
-}
-
-void walking_start(void)
-{
-  ram_data[DARWIN_WALK_CNTRL]|=WALK_STATUS;
-  m_Ctrl_Running=0x01;
-}
-
-void walking_stop(void)
-{
-  m_Ctrl_Running=0x00;
-}
-
-uint8_t is_walking(void)
-{
-  if(ram_data[DARWIN_WALK_CNTRL]&WALK_STATUS)
-    return 0x01;
-  else 
-    return 0x00;
-}
-
-// motion manager interface functions
-void walking_process(void)
-{
-  float x_swap,y_swap,z_swap,a_swap,b_swap,c_swap;
-  float x_move_r,y_move_r,z_move_r,a_move_r,b_move_r,c_move_r;
-  float x_move_l,y_move_l,z_move_l,a_move_l,b_move_l,c_move_l;
-  float pelvis_offset_r,pelvis_offset_l;
-//  float m_Body_Swing_Y,m_Body_Swing_Z;
-  float angle[14],ep[12];
-
-  if(m_Time==0)
-  {
-    update_param_time();
-    ram_data[DARWIN_WALK_CNTRL]&=(~WALK_PHASE);
-    ram_data[DARWIN_WALK_CNTRL]|=PHASE0;
-    if(m_Ctrl_Running==0x00)
-    {
-      if(m_X_Move_Amplitude==0 && m_Y_Move_Amplitude==0 && m_A_Move_Amplitude==0)
-        ram_data[DARWIN_WALK_CNTRL]&=(~WALK_STATUS);
-      else
-      {
-        ram_data[DARWIN_WALK_STEP_FW_BW]=0.0;
-        ram_data[DARWIN_WALK_STEP_LEFT_RIGHT]=0.0;
-        ram_data[DARWIN_WALK_STEP_DIRECTION]=0.0;
-      }
-    }
-  }
-  else if(m_Time>=(m_Phase_Time1-walking_period/2.0) && m_Time<(m_Phase_Time1+walking_period/2.0))
-  {
-    update_param_move();
-    ram_data[DARWIN_WALK_CNTRL]&=(~WALK_PHASE);
-    ram_data[DARWIN_WALK_CNTRL]|=PHASE1;
-  }
-  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;
-    ram_data[DARWIN_WALK_CNTRL]&=(~WALK_PHASE);
-    ram_data[DARWIN_WALK_CNTRL]|=PHASE2;
-    if(m_Ctrl_Running==0x00)
-    {
-      if(m_X_Move_Amplitude==0 && m_Y_Move_Amplitude==0 && m_A_Move_Amplitude==0)
-        ram_data[DARWIN_WALK_CNTRL]&=~WALK_STATUS;
-      else
-      {
-        ram_data[DARWIN_WALK_STEP_FW_BW]=0.0;
-        ram_data[DARWIN_WALK_STEP_LEFT_RIGHT]=0.0;
-        ram_data[DARWIN_WALK_STEP_DIRECTION]=0.0;
-      }
-    }
-  }
-  else if(m_Time>=(m_Phase_Time3-walking_period/2.0) && m_Time<(m_Phase_Time3+walking_period/2.0))
-  {
-    update_param_move();
-    ram_data[DARWIN_WALK_CNTRL]&=(~WALK_PHASE);
-    ram_data[DARWIN_WALK_CNTRL]|=PHASE3;
-  }
-  update_param_balance();
-
-  // Compute endpoints
-  x_swap=wsin(m_Time,m_X_Swap_PeriodTime,m_X_Swap_Phase_Shift,m_X_Swap_Amplitude,m_X_Swap_Amplitude_Shift);
-  y_swap=wsin(m_Time,m_Y_Swap_PeriodTime,m_Y_Swap_Phase_Shift,m_Y_Swap_Amplitude,m_Y_Swap_Amplitude_Shift);
-  z_swap=wsin(m_Time,m_Z_Swap_PeriodTime,m_Z_Swap_Phase_Shift,m_Z_Swap_Amplitude,m_Z_Swap_Amplitude_Shift);
-  a_swap=0;
-  b_swap=0;
-  c_swap=0;
-  if(m_Time<=m_SSP_Time_Start_L)
-  {
-    x_move_l=wsin(m_SSP_Time_Start_L,m_X_Move_PeriodTime,m_X_Move_Phase_Shift+2.0*PI/m_X_Move_PeriodTime*m_SSP_Time_Start_L,m_X_Move_Amplitude,m_X_Move_Amplitude_Shift);
-    y_move_l=wsin(m_SSP_Time_Start_L,m_Y_Move_PeriodTime,m_Y_Move_Phase_Shift+2.0*PI/m_Y_Move_PeriodTime*m_SSP_Time_Start_L,m_Y_Move_Amplitude,m_Y_Move_Amplitude_Shift);
-    z_move_l=wsin(m_SSP_Time_Start_L,m_Z_Move_PeriodTime,m_Z_Move_Phase_Shift+2.0*PI/m_Z_Move_PeriodTime*m_SSP_Time_Start_L,m_Z_Move_Amplitude,m_Z_Move_Amplitude_Shift);
-    c_move_l=wsin(m_SSP_Time_Start_L,m_A_Move_PeriodTime,m_A_Move_Phase_Shift+2.0*PI/m_A_Move_PeriodTime*m_SSP_Time_Start_L,m_A_Move_Amplitude,m_A_Move_Amplitude_Shift);
-    x_move_r=wsin(m_SSP_Time_Start_L,m_X_Move_PeriodTime,m_X_Move_Phase_Shift+2.0*PI/m_X_Move_PeriodTime*m_SSP_Time_Start_L,-m_X_Move_Amplitude,-m_X_Move_Amplitude_Shift);
-    y_move_r=wsin(m_SSP_Time_Start_L,m_Y_Move_PeriodTime,m_Y_Move_Phase_Shift+2.0*PI/m_Y_Move_PeriodTime*m_SSP_Time_Start_L,-m_Y_Move_Amplitude,-m_Y_Move_Amplitude_Shift);
-    z_move_r=wsin(m_SSP_Time_Start_R,m_Z_Move_PeriodTime,m_Z_Move_Phase_Shift+2.0*PI/m_Z_Move_PeriodTime*m_SSP_Time_Start_R,m_Z_Move_Amplitude,m_Z_Move_Amplitude_Shift);
-    c_move_r=wsin(m_SSP_Time_Start_L,m_A_Move_PeriodTime,m_A_Move_Phase_Shift+2.0*PI/m_A_Move_PeriodTime*m_SSP_Time_Start_L,-m_A_Move_Amplitude,-m_A_Move_Amplitude_Shift);
-    pelvis_offset_l=0;
-    pelvis_offset_r=0;
-  }
-  else if(m_Time<=m_SSP_Time_End_L)
-  {
-    x_move_l=wsin(m_Time,m_X_Move_PeriodTime,m_X_Move_Phase_Shift+2.0*PI/m_X_Move_PeriodTime*m_SSP_Time_Start_L,m_X_Move_Amplitude,m_X_Move_Amplitude_Shift);
-    y_move_l=wsin(m_Time,m_Y_Move_PeriodTime,m_Y_Move_Phase_Shift+2.0*PI/m_Y_Move_PeriodTime*m_SSP_Time_Start_L,m_Y_Move_Amplitude,m_Y_Move_Amplitude_Shift);
-    z_move_l=wsin(m_Time,m_Z_Move_PeriodTime,m_Z_Move_Phase_Shift+2.0*PI/m_Z_Move_PeriodTime*m_SSP_Time_Start_L,m_Z_Move_Amplitude,m_Z_Move_Amplitude_Shift);
-    c_move_l=wsin(m_Time,m_A_Move_PeriodTime,m_A_Move_Phase_Shift+2.0*PI/m_A_Move_PeriodTime*m_SSP_Time_Start_L,m_A_Move_Amplitude,m_A_Move_Amplitude_Shift);
-    x_move_r=wsin(m_Time,m_X_Move_PeriodTime,m_X_Move_Phase_Shift+2.0*PI/m_X_Move_PeriodTime*m_SSP_Time_Start_L,-m_X_Move_Amplitude,-m_X_Move_Amplitude_Shift);
-    y_move_r=wsin(m_Time,m_Y_Move_PeriodTime,m_Y_Move_Phase_Shift+2.0*PI/m_Y_Move_PeriodTime*m_SSP_Time_Start_L,-m_Y_Move_Amplitude,-m_Y_Move_Amplitude_Shift);
-    z_move_r=wsin(m_SSP_Time_Start_R,m_Z_Move_PeriodTime,m_Z_Move_Phase_Shift+2.0*PI/m_Z_Move_PeriodTime*m_SSP_Time_Start_R,m_Z_Move_Amplitude,m_Z_Move_Amplitude_Shift);
-    c_move_r=wsin(m_Time,m_A_Move_PeriodTime,m_A_Move_Phase_Shift+2.0*PI/m_A_Move_PeriodTime*m_SSP_Time_Start_L,-m_A_Move_Amplitude,-m_A_Move_Amplitude_Shift);
-    pelvis_offset_l=wsin(m_Time,m_Z_Move_PeriodTime,m_Z_Move_Phase_Shift+2.0*PI/m_Z_Move_PeriodTime*m_SSP_Time_Start_L,m_Pelvis_Swing/2.0,m_Pelvis_Swing/2.0);
-    pelvis_offset_r=wsin(m_Time,m_Z_Move_PeriodTime,m_Z_Move_Phase_Shift+2.0*PI/m_Z_Move_PeriodTime*m_SSP_Time_Start_L,-m_Pelvis_Offset/2.0,-m_Pelvis_Offset/2.0);
-  }
-  else if(m_Time<=m_SSP_Time_Start_R)
-  {
-    x_move_l=wsin(m_SSP_Time_End_L,m_X_Move_PeriodTime,m_X_Move_Phase_Shift+2.0*PI/m_X_Move_PeriodTime*m_SSP_Time_Start_L,m_X_Move_Amplitude,m_X_Move_Amplitude_Shift);
-    y_move_l=wsin(m_SSP_Time_End_L,m_Y_Move_PeriodTime,m_Y_Move_Phase_Shift+2.0*PI/m_Y_Move_PeriodTime*m_SSP_Time_Start_L,m_Y_Move_Amplitude,m_Y_Move_Amplitude_Shift);
-    z_move_l=wsin(m_SSP_Time_End_L,m_Z_Move_PeriodTime,m_Z_Move_Phase_Shift+2.0*PI/m_Z_Move_PeriodTime*m_SSP_Time_Start_L,m_Z_Move_Amplitude,m_Z_Move_Amplitude_Shift);
-    c_move_l=wsin(m_SSP_Time_End_L,m_A_Move_PeriodTime,m_A_Move_Phase_Shift+2.0*PI/m_A_Move_PeriodTime*m_SSP_Time_Start_L,m_A_Move_Amplitude,m_A_Move_Amplitude_Shift);
-    x_move_r=wsin(m_SSP_Time_End_L,m_X_Move_PeriodTime,m_X_Move_Phase_Shift+2.0*PI/m_X_Move_PeriodTime*m_SSP_Time_Start_L,-m_X_Move_Amplitude,-m_X_Move_Amplitude_Shift);
-    y_move_r=wsin(m_SSP_Time_End_L,m_Y_Move_PeriodTime,m_Y_Move_Phase_Shift+2.0*PI/m_Y_Move_PeriodTime*m_SSP_Time_Start_L,-m_Y_Move_Amplitude,-m_Y_Move_Amplitude_Shift);
-    z_move_r=wsin(m_SSP_Time_Start_R,m_Z_Move_PeriodTime,m_Z_Move_Phase_Shift+2.0*PI/m_Z_Move_PeriodTime*m_SSP_Time_Start_R,m_Z_Move_Amplitude,m_Z_Move_Amplitude_Shift);
-    c_move_r=wsin(m_SSP_Time_End_L,m_A_Move_PeriodTime,m_A_Move_Phase_Shift+2.0*PI/m_A_Move_PeriodTime*m_SSP_Time_Start_L,-m_A_Move_Amplitude,-m_A_Move_Amplitude_Shift);
-    pelvis_offset_l=0.0;
-    pelvis_offset_r=0.0;
-  }
-  else if(m_Time<=m_SSP_Time_End_R)
-  {
-    x_move_l=wsin(m_Time,m_X_Move_PeriodTime,m_X_Move_Phase_Shift+2.0*PI/m_X_Move_PeriodTime*m_SSP_Time_Start_R+PI,m_X_Move_Amplitude,m_X_Move_Amplitude_Shift);
-    y_move_l=wsin(m_Time,m_Y_Move_PeriodTime,m_Y_Move_Phase_Shift+2.0*PI/m_Y_Move_PeriodTime*m_SSP_Time_Start_R+PI,m_Y_Move_Amplitude,m_Y_Move_Amplitude_Shift);
-    z_move_l=wsin(m_SSP_Time_End_L,m_Z_Move_PeriodTime,m_Z_Move_Phase_Shift+2.0*PI/m_Z_Move_PeriodTime*m_SSP_Time_Start_L,m_Z_Move_Amplitude,m_Z_Move_Amplitude_Shift);
-    c_move_l=wsin(m_Time,m_A_Move_PeriodTime,m_A_Move_Phase_Shift+2.0*PI/m_A_Move_PeriodTime*m_SSP_Time_Start_R+PI,m_A_Move_Amplitude,m_A_Move_Amplitude_Shift);
-    x_move_r=wsin(m_Time,m_X_Move_PeriodTime,m_X_Move_Phase_Shift+2.0*PI/m_X_Move_PeriodTime*m_SSP_Time_Start_R+PI,-m_X_Move_Amplitude,-m_X_Move_Amplitude_Shift);
-    y_move_r=wsin(m_Time,m_Y_Move_PeriodTime,m_Y_Move_Phase_Shift+2.0*PI/m_Y_Move_PeriodTime*m_SSP_Time_Start_R+PI,-m_Y_Move_Amplitude,-m_Y_Move_Amplitude_Shift);
-    z_move_r=wsin(m_Time,m_Z_Move_PeriodTime,m_Z_Move_Phase_Shift+2.0*PI/m_Z_Move_PeriodTime*m_SSP_Time_Start_R,m_Z_Move_Amplitude,m_Z_Move_Amplitude_Shift);
-    c_move_r=wsin(m_Time,m_A_Move_PeriodTime,m_A_Move_Phase_Shift+2.0*PI/m_A_Move_PeriodTime*m_SSP_Time_Start_R+PI,-m_A_Move_Amplitude,-m_A_Move_Amplitude_Shift);
-    pelvis_offset_l=wsin(m_Time,m_Z_Move_PeriodTime,m_Z_Move_Phase_Shift+2.0*PI/m_Z_Move_PeriodTime*m_SSP_Time_Start_R,m_Pelvis_Offset/2,m_Pelvis_Offset/2);
-    pelvis_offset_r=wsin(m_Time,m_Z_Move_PeriodTime,m_Z_Move_Phase_Shift+2.0*PI/m_Z_Move_PeriodTime*m_SSP_Time_Start_R,-m_Pelvis_Swing/2,-m_Pelvis_Swing/2);
-  }
-  else
-  {
-    x_move_l=wsin(m_SSP_Time_End_R,m_X_Move_PeriodTime,m_X_Move_Phase_Shift+2.0*PI/m_X_Move_PeriodTime*m_SSP_Time_Start_R+PI,m_X_Move_Amplitude,m_X_Move_Amplitude_Shift);
-    y_move_l=wsin(m_SSP_Time_End_R,m_Y_Move_PeriodTime,m_Y_Move_Phase_Shift+2.0*PI/m_Y_Move_PeriodTime*m_SSP_Time_Start_R+PI,m_Y_Move_Amplitude,m_Y_Move_Amplitude_Shift);
-    z_move_l=wsin(m_SSP_Time_End_L,m_Z_Move_PeriodTime,m_Z_Move_Phase_Shift+2.0*PI/m_Z_Move_PeriodTime*m_SSP_Time_Start_L,m_Z_Move_Amplitude,m_Z_Move_Amplitude_Shift);
-    c_move_l=wsin(m_SSP_Time_End_R,m_A_Move_PeriodTime,m_A_Move_Phase_Shift+2.0*PI/m_A_Move_PeriodTime*m_SSP_Time_Start_R+PI,m_A_Move_Amplitude,m_A_Move_Amplitude_Shift);
-    x_move_r=wsin(m_SSP_Time_End_R,m_X_Move_PeriodTime,m_X_Move_Phase_Shift+2.0*PI/m_X_Move_PeriodTime*m_SSP_Time_Start_R+PI,-m_X_Move_Amplitude,-m_X_Move_Amplitude_Shift);
-    y_move_r=wsin(m_SSP_Time_End_R,m_Y_Move_PeriodTime,m_Y_Move_Phase_Shift+2.0*PI/m_Y_Move_PeriodTime*m_SSP_Time_Start_R+PI,-m_Y_Move_Amplitude,-m_Y_Move_Amplitude_Shift);
-    z_move_r=wsin(m_SSP_Time_End_R,m_Z_Move_PeriodTime,m_Z_Move_Phase_Shift+2.0*PI/m_Z_Move_PeriodTime*m_SSP_Time_Start_R,m_Z_Move_Amplitude,m_Z_Move_Amplitude_Shift);
-    c_move_r=wsin(m_SSP_Time_End_R,m_A_Move_PeriodTime,m_A_Move_Phase_Shift+2.0*PI/m_A_Move_PeriodTime*m_SSP_Time_Start_R+PI,-m_A_Move_Amplitude,-m_A_Move_Amplitude_Shift);
-    pelvis_offset_l=0.0;
-    pelvis_offset_r=0.0;
-  }
-  a_move_l=0.0;
-  b_move_l=0.0;
-  a_move_r=0.0;
-  b_move_r=0.0;
-
-  ep[0]=x_swap+x_move_r+m_X_Offset;
-  ep[1]=y_swap+y_move_r-m_Y_Offset / 2.0;
-  ep[2]=z_swap+z_move_r+m_Z_Offset;
-  ep[3]=a_swap+a_move_r-m_R_Offset / 2.0;
-  ep[4]=b_swap+b_move_r+m_P_Offset;
-  ep[5]=c_swap+c_move_r-m_A_Offset / 2.0;
-  ep[6]=x_swap+x_move_l+m_X_Offset;
-  ep[7]=y_swap+y_move_l+m_Y_Offset / 2.0;
-  ep[8]=z_swap+z_move_l+m_Z_Offset;
-  ep[9]=a_swap+a_move_l+m_R_Offset / 2.0;
-  ep[10]=b_swap+b_move_l+m_P_Offset;
-  ep[11]=c_swap+c_move_l+m_A_Offset / 2.0;
-
-  // Compute body swing
-/*  if(m_Time<=m_SSP_Time_End_L)
-  {
-    m_Body_Swing_Y=-ep[7];
-    m_Body_Swing_Z=ep[8];
-  }
-  else
-  {
-    m_Body_Swing_Y=-ep[1];
-    m_Body_Swing_Z=ep[2];
-  }
-  m_Body_Swing_Z-=DARWIN_LEG_LENGTH;*/
-
-  // Compute arm swing
-  if(m_X_Move_Amplitude==0)
-  {
-    angle[12]=0; // Right
-    angle[13]=0; // Left
-  }
-  else
-  {
-    angle[12]=wsin(m_Time,m_PeriodTime,0.0,-m_X_Move_Amplitude*m_Arm_Swing_Gain,0.0);
-    angle[13]=wsin(m_Time,m_PeriodTime,0.0,m_X_Move_Amplitude*m_Arm_Swing_Gain,0.0);
-  }
-
-  if(ram_data[DARWIN_WALK_CNTRL]&WALK_STATUS)
-  {
-    m_Time += walking_period;
-    if(m_Time >= m_PeriodTime)
-      m_Time = 0;
-  }
-
-  // Compute angles with the inverse kinematics
-  if((darwin_leg_ik(&angle[0],ep[0],ep[1],ep[2],ep[3],ep[4],ep[5])!=1) || 
-     (darwin_leg_ik(&angle[6],ep[6],ep[7],ep[8],ep[9],ep[10],ep[11])!=1))
-    return;// Do not use angles
-
-  // Compute motor value
-  if(manager_get_module(R_HIP_YAW)==MM_WALKING)
-    manager_current_angles[R_HIP_YAW]=((-180.0*angle[0])/PI)*65536.0;
-  if(manager_get_module(R_HIP_ROLL)==MM_WALKING)
-    manager_current_angles[R_HIP_ROLL]=((-180.0*(angle[1]+pelvis_offset_r))/PI)*65536.0;
-  if(manager_get_module(R_HIP_PITCH)==MM_WALKING)
-    manager_current_angles[R_HIP_PITCH]=((180.0*angle[2])/PI-m_Hip_Pitch_Offset)*65536.0;
-  if(manager_get_module(R_KNEE)==MM_WALKING)
-    manager_current_angles[R_KNEE]=((180.0*angle[3])/PI)*65536.0;
-  if(manager_get_module(R_ANKLE_PITCH)==MM_WALKING)
-    manager_current_angles[R_ANKLE_PITCH]=((-180.0*angle[4])/PI)*65536.0;
-  if(manager_get_module(R_ANKLE_ROLL)==MM_WALKING)
-    manager_current_angles[R_ANKLE_ROLL]=((180.0*angle[5])/PI)*65536.0;
-  if(manager_get_module(L_HIP_YAW)==MM_WALKING)
-    manager_current_angles[L_HIP_YAW]=((-180.0*angle[6])/PI)*65536.0;
-  if(manager_get_module(L_HIP_ROLL)==MM_WALKING)
-    manager_current_angles[L_HIP_ROLL]=((-180.0*(angle[7]+pelvis_offset_l))/PI)*65536.0;
-  if(manager_get_module(L_HIP_PITCH)==MM_WALKING)
-    manager_current_angles[L_HIP_PITCH]=((-180.0*angle[8])/PI+m_Hip_Pitch_Offset)*65536.0;
-  if(manager_get_module(L_KNEE)==MM_WALKING)
-    manager_current_angles[L_KNEE]=((-180.0*angle[9])/PI)*65536.0;
-  if(manager_get_module(L_ANKLE_PITCH)==MM_WALKING)
-    manager_current_angles[L_ANKLE_PITCH]=((180.0*angle[10])/PI)*65536.0;
-  if(manager_get_module(L_ANKLE_ROLL)==MM_WALKING)
-    manager_current_angles[L_ANKLE_ROLL]=((180.0*angle[11])/PI)*65536.0;
-  if(manager_get_module(R_SHOULDER_PITCH)==MM_WALKING)
-    manager_current_angles[R_SHOULDER_PITCH]=(angle[12]-48.345)*65536.0;
-  if(manager_get_module(L_SHOULDER_PITCH)==MM_WALKING)
-    manager_current_angles[L_SHOULDER_PITCH]=(-angle[13]+41.313)*65536.0;
-}
-
-// operation functions
-uint8_t walking_in_range(unsigned short int address, unsigned short int length)
-{
-  if(ram_in_window(WALK_BASE_ADDRESS,WALK_MEM_LENGTH,address,length) ||
-     ram_in_window(WALK_EEPROM_ADDRESS,WALK_EEPROM_LENGTH,address,length))
-    return 0x01;
-  else
-    return 0x00;
-}
-
-void walking_process_read_cmd(unsigned short int address,unsigned short int length,unsigned char *data)
-{
-
-}
-
-void walking_process_write_cmd(unsigned short int address,unsigned short int length,unsigned char *data)
-{
-  uint16_t i;
-
-  // walk control
-  if(ram_in_range(DARWIN_WALK_CNTRL,address,length))
-  {
-    if(data[DARWIN_WALK_CNTRL-address]&WALK_START)
-      walking_start();
-    if(data[DARWIN_WALK_CNTRL-address]&WALK_STOP)
-      walking_stop();
-  }
-  if(ram_in_range(DARWIN_WALK_STEP_FW_BW,address,length))
-    ram_data[DARWIN_WALK_STEP_FW_BW]=data[DARWIN_WALK_STEP_FW_BW-address];
-  if(ram_in_range(DARWIN_WALK_STEP_LEFT_RIGHT,address,length))
-    ram_data[DARWIN_WALK_STEP_LEFT_RIGHT]=data[DARWIN_WALK_STEP_LEFT_RIGHT-address];
-  if(ram_in_range(DARWIN_WALK_STEP_DIRECTION,address,length))
-    ram_data[DARWIN_WALK_STEP_DIRECTION]=data[DARWIN_WALK_STEP_DIRECTION-address];
-  // walk parameters
-  for(i=WALK_EEPROM_ADDRESS;i<=WALK_EEPROM_ADDRESS+WALK_EEPROM_LENGTH;i++)
-    if(ram_in_range(i,address,length))
-      ram_data[i]=data[i-address];
-}
-