diff --git a/dynamixel_manager/src/modules/head_tracking.c b/dynamixel_manager/src/modules/head_tracking.c
index 72e786153fca490906bac68290ba6067fa835f7f..7612930fef0a6d81b89b4cd10fa9d02c1f5dc6a9 100644
--- a/dynamixel_manager/src/modules/head_tracking.c
+++ b/dynamixel_manager/src/modules/head_tracking.c
@@ -22,18 +22,18 @@ void head_tracking_write_cmd(THeadMModule *module,unsigned short int address,uns
     if(ram_in_range(module->eeprom_base_address+HEAD_TRACKING_PAN_P_GAIN_OFFSET+1,address,length))
       kp=(kp&0x00FF)|(data[HEAD_TRACKING_PAN_P_GAIN_OFFSET+1-eeprom_offset]<<8);
     if(ram_in_range(module->eeprom_base_address+HEAD_TRACKING_PAN_I_GAIN_OFFSET,address,length))
-      ki=(kp&0xFF00)|data[HEAD_TRACKING_PAN_I_GAIN_OFFSET-eeprom_offset];
+      ki=(ki&0xFF00)|data[HEAD_TRACKING_PAN_I_GAIN_OFFSET-eeprom_offset];
     if(ram_in_range(module->eeprom_base_address+HEAD_TRACKING_PAN_I_GAIN_OFFSET+1,address,length))
-      ki=(kp&0x00FF)|(data[HEAD_TRACKING_PAN_I_GAIN_OFFSET+1-eeprom_offset]<<8);
+      ki=(ki&0x00FF)|(data[HEAD_TRACKING_PAN_I_GAIN_OFFSET+1-eeprom_offset]<<8);
     if(ram_in_range(module->eeprom_base_address+HEAD_TRACKING_PAN_D_GAIN_OFFSET,address,length))
-      kd=(kp&0xFF00)|data[HEAD_TRACKING_PAN_D_GAIN_OFFSET-eeprom_offset];
+      kd=(kd&0xFF00)|data[HEAD_TRACKING_PAN_D_GAIN_OFFSET-eeprom_offset];
     if(ram_in_range(module->eeprom_base_address+HEAD_TRACKING_PAN_D_GAIN_OFFSET+1,address,length))
-      kd=(kp&0x00FF)|(data[HEAD_TRACKING_PAN_D_GAIN_OFFSET+1-eeprom_offset]<<8);
+      kd=(kd&0x00FF)|(data[HEAD_TRACKING_PAN_D_GAIN_OFFSET+1-eeprom_offset]<<8);
     if(ram_in_range(module->eeprom_base_address+HEAD_TRACKING_PAN_I_CLAMP_OFFSET,address,length))
       i_clamp=(kp&0xFF00)|data[HEAD_TRACKING_PAN_I_CLAMP_OFFSET-eeprom_offset];
     if(ram_in_range(module->eeprom_base_address+HEAD_TRACKING_PAN_I_CLAMP_OFFSET+1,address,length))
       i_clamp=(kp&0x00FF)|(data[HEAD_TRACKING_PAN_I_CLAMP_OFFSET+1-eeprom_offset]<<8);
-    head_tracking_set_pan_pid(module,kp,kd,ki,i_clamp);
+    head_tracking_set_pan_pid(module,kp,ki,kd,i_clamp);
   }
   if(ram_in_window(module->eeprom_base_address+HEAD_TRACKING_TILT_P_GAIN_OFFSET,8,address,length))
   {
@@ -43,18 +43,18 @@ void head_tracking_write_cmd(THeadMModule *module,unsigned short int address,uns
     if(ram_in_range(module->eeprom_base_address+HEAD_TRACKING_TILT_P_GAIN_OFFSET+1,address,length))
       kp=(kp&0x00FF)|(data[HEAD_TRACKING_TILT_P_GAIN_OFFSET+1-eeprom_offset]<<8);
     if(ram_in_range(module->eeprom_base_address+HEAD_TRACKING_TILT_I_GAIN_OFFSET,address,length))
-      ki=(kp&0xFF00)|data[HEAD_TRACKING_TILT_I_GAIN_OFFSET-eeprom_offset];
+      ki=(ki&0xFF00)|data[HEAD_TRACKING_TILT_I_GAIN_OFFSET-eeprom_offset];
     if(ram_in_range(module->eeprom_base_address+HEAD_TRACKING_TILT_I_GAIN_OFFSET+1,address,length))
-      ki=(kp&0x00FF)|(data[HEAD_TRACKING_TILT_I_GAIN_OFFSET+1-eeprom_offset]<<8);
+      ki=(ki&0x00FF)|(data[HEAD_TRACKING_TILT_I_GAIN_OFFSET+1-eeprom_offset]<<8);
     if(ram_in_range(module->eeprom_base_address+HEAD_TRACKING_TILT_D_GAIN_OFFSET,address,length))
-      kd=(kp&0xFF00)|data[HEAD_TRACKING_TILT_D_GAIN_OFFSET-eeprom_offset];
+      kd=(kd&0xFF00)|data[HEAD_TRACKING_TILT_D_GAIN_OFFSET-eeprom_offset];
     if(ram_in_range(module->eeprom_base_address+HEAD_TRACKING_TILT_D_GAIN_OFFSET+1,address,length))
-      kd=(kp&0x00FF)|(data[HEAD_TRACKING_TILT_D_GAIN_OFFSET+1-eeprom_offset]<<8);
+      kd=(kd&0x00FF)|(data[HEAD_TRACKING_TILT_D_GAIN_OFFSET+1-eeprom_offset]<<8);
     if(ram_in_range(module->eeprom_base_address+HEAD_TRACKING_TILT_I_CLAMP_OFFSET,address,length))
       i_clamp=(kp&0xFF00)|data[HEAD_TRACKING_TILT_I_CLAMP_OFFSET-eeprom_offset];
     if(ram_in_range(module->eeprom_base_address+HEAD_TRACKING_TILT_I_CLAMP_OFFSET+1,address,length))
       i_clamp=(kp&0x00FF)|(data[HEAD_TRACKING_TILT_I_CLAMP_OFFSET+1-eeprom_offset]<<8);
-    head_tracking_set_tilt_pid(module,kp,kd,ki,i_clamp);
+    head_tracking_set_tilt_pid(module,kp,ki,kd,i_clamp);
   }
   if(ram_in_range(module->eeprom_base_address+HEAD_TRACKING_PAN_SERVO_ID_OFFSET,address,length))
     head_tracking_set_pan_servo_id(module,data[HEAD_TRACKING_PAN_SERVO_ID_OFFSET-eeprom_offset]);
@@ -239,8 +239,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_pan_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)