Commit 1a306833 authored by Sergi Hernandez's avatar Sergi Hernandez
Browse files

Solved some bugs in the dynamixel register write operation.

Changed the lont int variables to long long int to avoid undesired overflow.
parent e2f712ae
......@@ -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
}
......
......@@ -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)
......
Markdown is supported
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