diff --git a/dynamixel_manager/include/modules/head_tracking.h b/dynamixel_manager/include/modules/head_tracking.h
index b4e9a3d0d24c5644ff71b11637bac5f36b8aa441..fe35b5c8985fc5e0bc50589e36291bddd54798cd 100644
--- a/dynamixel_manager/include/modules/head_tracking.h
+++ b/dynamixel_manager/include/modules/head_tracking.h
@@ -13,12 +13,12 @@ typedef struct{
   unsigned short int kp;
   unsigned short int ki;
   unsigned short int kd;
-  long int prev_error;// 48|16
-  long int integral_part;
-  long int integral_clamp;
-  long int max_angle;
-  long int min_angle;
-  long int target_angle;
+  long long int prev_error;// 48|16
+  long long int integral_part;
+  long long int integral_clamp;
+  long long int max_angle;
+  long long int min_angle;
+  long long int target_angle;
 }TJointControl;
  
 typedef struct{
@@ -31,29 +31,29 @@ typedef struct{
   unsigned char pan_servo_id;
   TJointControl tracking_tilt;
   unsigned char tilt_servo_id;
-  long int tracking_period;
+  long long int tracking_period;
   short int tracking_period_ms;
   unsigned char tracking_enabled;
 }THeadMModule;
 
 // public functions
-unsigned char head_tracking_init(THeadMModule *joint,TMemory *memory,unsigned short int ram_base_address,unsigned short int eeprom_base_address);
+unsigned char head_tracking_init(THeadMModule *head,TMemory *memory,unsigned short int ram_base_address,unsigned short int eeprom_base_address);
 TMotionModule *head_tracking_get_module(THeadMModule *head);
-void head_tracking_start(THeadMModule *joint);
-void head_tracking_stop(THeadMModule *joint);
-unsigned char head_is_tracking(THeadMModule *joint);
-void head_tracking_set_pan_range(THeadMModule *joint,short int max_angle,short int min_angle);
-void head_tracking_get_pan_range(THeadMModule *joint,short int *max_angle,short int *min_angle);
-void head_tracking_set_pan_pid(THeadMModule *joint,unsigned short int kp,unsigned short int ki,unsigned short int kd, unsigned short int i_clamp);
-void head_tracking_get_pan_pid(THeadMModule *joint,unsigned short int *kp,unsigned short int *ki,unsigned short int *kd, unsigned short int *i_clamp);
-void head_tracking_set_tilt_range(THeadMModule *joint,short int max_angle,short int min_angle);
-void head_tracking_get_tilt_range(THeadMModule *joint,short int *max_angle,short int *min_angle);
-void head_tracking_set_tilt_pid(THeadMModule *joint,unsigned short int kp,unsigned short int ki,unsigned short int kd, unsigned short int i_clamp);
-void head_tracking_get_tilt_pid(THeadMModule *joint,unsigned short int *kp,unsigned short int *ki,unsigned short int *kd, unsigned short int *i_clamp);
-void head_tracking_set_pan_pan(THeadMModule *joint,short int target);
-void head_tracking_set_tilt_target(THeadMModule *joint,short int target);
-void head_tracking_set_pan_servo_id(THeadMModule *joint,unsigned char servo_id);
-void head_tracking_set_tilt_servo_id(THeadMModule *joint,unsigned char servo_id);
+void head_tracking_start(THeadMModule *head);
+void head_tracking_stop(THeadMModule *head);
+unsigned char head_is_tracking(THeadMModule *head);
+void head_tracking_set_pan_range(THeadMModule *head,short int max_angle,short int min_angle);
+void head_tracking_get_pan_range(THeadMModule *head,short int *max_angle,short int *min_angle);
+void head_tracking_set_pan_pid(THeadMModule *head,unsigned short int kp,unsigned short int ki,unsigned short int kd, unsigned short int i_clamp);
+void head_tracking_get_pan_pid(THeadMModule *head,unsigned short int *kp,unsigned short int *ki,unsigned short int *kd, unsigned short int *i_clamp);
+void head_tracking_set_tilt_range(THeadMModule *head,short int max_angle,short int min_angle);
+void head_tracking_get_tilt_range(THeadMModule *head,short int *max_angle,short int *min_angle);
+void head_tracking_set_tilt_pid(THeadMModule *head,unsigned short int kp,unsigned short int ki,unsigned short int kd, unsigned short int i_clamp);
+void head_tracking_get_tilt_pid(THeadMModule *head,unsigned short int *kp,unsigned short int *ki,unsigned short int *kd, unsigned short int *i_clamp);
+void head_tracking_set_pan_pan(THeadMModule *head,short int target);
+void head_tracking_set_tilt_target(THeadMModule *head,short int target);
+void head_tracking_set_pan_servo_id(THeadMModule *head,unsigned char servo_id);
+void head_tracking_set_tilt_servo_id(THeadMModule *head,unsigned char servo_id);
 
 #ifdef __cplusplus
 }
diff --git a/dynamixel_manager/src/modules/head_tracking.c b/dynamixel_manager/src/modules/head_tracking.c
index 7612930fef0a6d81b89b4cd10fa9d02c1f5dc6a9..6b34b82ba895ada4b3ae332829b98dcfc1ef406a 100644
--- a/dynamixel_manager/src/modules/head_tracking.c
+++ b/dynamixel_manager/src/modules/head_tracking.c
@@ -78,9 +78,9 @@ void head_tracking_write_cmd(THeadMModule *module,unsigned short int address,uns
     if(ram_in_range(module->ram_base_address+HEAD_TRACKING_MAX_PAN_OFFSET+1,address,length))
       max=(max&0x00FF)|(data[HEAD_TRACKING_MAX_PAN_OFFSET+1-ram_offset]<<8);
     if(ram_in_range(module->ram_base_address+HEAD_TRACKING_MIN_PAN_OFFSET,address,length)) 
-      max=(max&0xFF00)|data[HEAD_TRACKING_MIN_PAN_OFFSET-ram_offset];
+      min=(min&0xFF00)|data[HEAD_TRACKING_MIN_PAN_OFFSET-ram_offset];
     if(ram_in_range(module->ram_base_address+HEAD_TRACKING_MIN_PAN_OFFSET+1,address,length))
-      max=(max&0x00FF)|(data[HEAD_TRACKING_MIN_PAN_OFFSET+1-ram_offset]<<8);
+      min=(min&0x00FF)|(data[HEAD_TRACKING_MIN_PAN_OFFSET+1-ram_offset]<<8);
     head_tracking_set_pan_range(module,max,min);
   }
   if(ram_in_window(module->ram_base_address+HEAD_TRACKING_MAX_TILT_OFFSET,4,address,length))
@@ -91,9 +91,9 @@ void head_tracking_write_cmd(THeadMModule *module,unsigned short int address,uns
     if(ram_in_range(module->ram_base_address+HEAD_TRACKING_MAX_TILT_OFFSET+1,address,length))
       max=(max&0x00FF)|(data[HEAD_TRACKING_MAX_TILT_OFFSET+1-ram_offset]<<8);
     if(ram_in_range(module->ram_base_address+HEAD_TRACKING_MIN_TILT_OFFSET,address,length)) 
-      max=(max&0xFF00)|data[HEAD_TRACKING_MIN_TILT_OFFSET-ram_offset];
+      min=(min&0xFF00)|data[HEAD_TRACKING_MIN_TILT_OFFSET-ram_offset];
     if(ram_in_range(module->ram_base_address+HEAD_TRACKING_MIN_TILT_OFFSET+1,address,length))
-      max=(max&0x00FF)|(data[HEAD_TRACKING_MIN_TILT_OFFSET+1-ram_offset]<<8);
+      min=(min&0x00FF)|(data[HEAD_TRACKING_MIN_TILT_OFFSET+1-ram_offset]<<8);
     head_tracking_set_tilt_range(module,max,min);
   }
   if(ram_in_window(module->ram_base_address+HEAD_TRACKING_PAN_TARGET_OFFSET,2,address,length))
@@ -107,7 +107,7 @@ void head_tracking_write_cmd(THeadMModule *module,unsigned short int address,uns
     if(ram_in_range(module->ram_base_address+HEAD_TRACKING_PAN_TARGET_OFFSET+1,address,length))
     {
       target=(target&0x00FF)|(data[HEAD_TRACKING_PAN_TARGET_OFFSET+1-ram_offset]<<8);
-      module->memory->data[module->ram_base_address+HEAD_TRACKING_PAN_TARGET_OFFSET]=data[HEAD_TRACKING_PAN_TARGET_OFFSET-ram_offset];
+      module->memory->data[module->ram_base_address+HEAD_TRACKING_PAN_TARGET_OFFSET+1]=data[HEAD_TRACKING_PAN_TARGET_OFFSET+1-ram_offset];
     }
     module->tracking_pan.target_angle=(target<<9);
   }
@@ -122,7 +122,7 @@ void head_tracking_write_cmd(THeadMModule *module,unsigned short int address,uns
     if(ram_in_range(module->ram_base_address+HEAD_TRACKING_TILT_TARGET_OFFSET+1,address,length))
     {
       target=(target&0x00FF)|(data[HEAD_TRACKING_TILT_TARGET_OFFSET+1-ram_offset]<<8);
-      module->memory->data[module->ram_base_address+HEAD_TRACKING_TILT_TARGET_OFFSET]=data[HEAD_TRACKING_TILT_TARGET_OFFSET-ram_offset];
+      module->memory->data[module->ram_base_address+HEAD_TRACKING_TILT_TARGET_OFFSET+1]=data[HEAD_TRACKING_TILT_TARGET_OFFSET+1-ram_offset];
     }
     module->tracking_tilt.target_angle=(target<<9);
   }
@@ -151,24 +151,23 @@ void head_tracking_set_module(void *module,unsigned char servo_id)
 void head_tracking_pre_process(void *module)
 {
   THeadMModule *head=(THeadMModule *)module;
-  long int pan_error, tilt_error;
-  long int derivative_pan=0, derivative_tilt=0;
+  signed long long int pan_error, tilt_error;
+  signed long long int derivative_pan=0, derivative_tilt=0;
 
   if(head->tracking_enabled)
   {
-    head->memory->data[head->ram_base_address+HEAD_TRACKING_CONTROL_OFFSET]|=HEAD_TRACKING_RUNNING;
     if(mmanager_get_module(head->mmodule.manager,head->pan_servo_id)==MM_HEAD)
     {
-      pan_error=head->tracking_pan.target_angle-head->mmodule.manager->servo_values[head->pan_servo_id].target_angle;
+      pan_error=head->tracking_pan.target_angle-head->mmodule.manager->servo_values[head->pan_servo_id].current_angle;
       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;
-      head->mmodule.manager->servo_values[head->pan_servo_id].target_angle+=((head->tracking_pan.kp*pan_error)>>16);
-      head->mmodule.manager->servo_values[head->pan_servo_id].target_angle+=((head->tracking_pan.ki*head->tracking_pan.integral_part)>>16);      
-      head->mmodule.manager->servo_values[head->pan_servo_id].target_angle+=((head->tracking_pan.kd*derivative_pan)>>16);      
+      head->mmodule.manager->servo_values[head->pan_servo_id].target_angle+=((signed long long int)head->tracking_pan.kp*pan_error)>>16;
+      head->mmodule.manager->servo_values[head->pan_servo_id].target_angle+=((signed long long int)head->tracking_pan.ki*head->tracking_pan.integral_part)>>16;      
+      head->mmodule.manager->servo_values[head->pan_servo_id].target_angle+=((signed long long int)head->tracking_pan.kd*derivative_pan)>>16;      
       if(head->mmodule.manager->servo_values[head->pan_servo_id].target_angle>head->tracking_pan.max_angle)
         head->mmodule.manager->servo_values[head->pan_servo_id].target_angle=head->tracking_pan.max_angle;
       else if(head->mmodule.manager->servo_values[head->pan_servo_id].target_angle<head->tracking_pan.min_angle)
@@ -177,16 +176,16 @@ void head_tracking_pre_process(void *module)
     }
     if(mmanager_get_module(head->mmodule.manager,head->tilt_servo_id)==MM_HEAD)
     { 
-      tilt_error=head->tracking_tilt.target_angle-head->mmodule.manager->servo_values[head->tilt_servo_id].target_angle;
+      tilt_error=head->tracking_tilt.target_angle-head->mmodule.manager->servo_values[head->tilt_servo_id].current_angle;
       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;
-      head->mmodule.manager->servo_values[head->tilt_servo_id].target_angle+=((head->tracking_tilt.kp*tilt_error)>>16);
-      head->mmodule.manager->servo_values[head->tilt_servo_id].target_angle+=((head->tracking_tilt.ki*head->tracking_tilt.integral_part)>>16);
-      head->mmodule.manager->servo_values[head->tilt_servo_id].target_angle+=((head->tracking_tilt.kd*derivative_tilt)>>16);
+      head->mmodule.manager->servo_values[head->tilt_servo_id].target_angle+=((signed long long int)head->tracking_tilt.kp*tilt_error)>>16;
+      head->mmodule.manager->servo_values[head->tilt_servo_id].target_angle+=((signed long long int)head->tracking_tilt.ki*head->tracking_tilt.integral_part)>>16;
+      head->mmodule.manager->servo_values[head->tilt_servo_id].target_angle+=((signed long long int)head->tracking_tilt.kd*derivative_tilt)>>16;
       if(head->mmodule.manager->servo_values[head->tilt_servo_id].target_angle>head->tracking_tilt.max_angle)
         head->mmodule.manager->servo_values[head->tilt_servo_id].target_angle=head->tracking_tilt.max_angle;
       else if(head->mmodule.manager->servo_values[head->tilt_servo_id].target_angle<head->tracking_tilt.min_angle)
@@ -239,8 +238,8 @@ void head_tracking_start(THeadMModule *head)
 {
   head->tracking_enabled=0x01;
   head->memory->data[head->ram_base_address+HEAD_TRACKING_CONTROL_OFFSET]|=HEAD_TRACKING_RUNNING;
-  head_tracking_set_pan_range(head,head->mmodule.manager->servo_configs[head->pan_servo_id]->ccw_angle_limit>>9,head->mmodule.manager->servo_configs[head->pan_servo_id]->cw_angle_limit>>9);
-  head_tracking_set_tilt_range(head,head->mmodule.manager->servo_configs[head->tilt_servo_id]->ccw_angle_limit>>9,head->mmodule.manager->servo_configs[head->tilt_servo_id]->cw_angle_limit>>9);
+//  head_tracking_set_pan_range(head,head->mmodule.manager->servo_configs[head->pan_servo_id]->ccw_angle_limit>>9,head->mmodule.manager->servo_configs[head->pan_servo_id]->cw_angle_limit>>9);
+//  head_tracking_set_tilt_range(head,head->mmodule.manager->servo_configs[head->tilt_servo_id]->ccw_angle_limit>>9,head->mmodule.manager->servo_configs[head->tilt_servo_id]->cw_angle_limit>>9);
 }
 
 void head_tracking_stop(THeadMModule *head)