Skip to content
Snippets Groups Projects
Commit bb00112f authored by Sergi Hernandez's avatar Sergi Hernandez
Browse files

Solved some minor bugs of the head tracking module.

parent 6b6fe086
No related branches found
No related tags found
No related merge requests found
Hide whitespace changes
Inline Side-by-side
dynamixel_manager/src/modules/head_tracking.c
+ 12
− 12
View file @ bb00112f
...@@ -22,18 +22,18 @@ void head_tracking_write_cmd(THeadMModule *module,unsigned short int address,uns ...@@ -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)) 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); 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)) 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)) 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)) 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)) 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)) 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]; 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)) 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); 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)) 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 ...@@ -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)) 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); 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)) 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)) 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)) 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)) 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)) 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]; 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)) 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); 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)) 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]); 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) ...@@ -239,8 +239,8 @@ void head_tracking_start(THeadMModule *head)
{ {
head->tracking_enabled=0x01; head->tracking_enabled=0x01;
head->memory->data[head->ram_base_address+HEAD_TRACKING_CONTROL_OFFSET]|=HEAD_TRACKING_RUNNING; 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->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_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) void head_tracking_stop(THeadMModule *head)
......
0% or .
You are about to add 0 people to the discussion. Proceed with caution.
Finish editing this message first!
Please register or to comment