Skip to content
Snippets Groups Projects
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
No related branches found
No related tags found
No related merge requests found
Hide whitespace changes
Inline Side-by-side
dynamixel_manager/include/modules/head_tracking.h
+ 23
− 23
View file @ 1a306833
...@@ -13,12 +13,12 @@ typedef struct{ ...@@ -13,12 +13,12 @@ typedef struct{
unsigned short int kp; unsigned short int kp;
unsigned short int ki; unsigned short int ki;
unsigned short int kd; unsigned short int kd;
long int prev_error;// 48|16 long long int prev_error;// 48|16
long int integral_part; long long int integral_part;
long int integral_clamp; long long int integral_clamp;
long int max_angle; long long int max_angle;
long int min_angle; long long int min_angle;
long int target_angle; long long int target_angle;
}TJointControl; }TJointControl;
typedef struct{ typedef struct{
...@@ -31,29 +31,29 @@ typedef struct{ ...@@ -31,29 +31,29 @@ typedef struct{
unsigned char pan_servo_id; unsigned char pan_servo_id;
TJointControl tracking_tilt; TJointControl tracking_tilt;
unsigned char tilt_servo_id; unsigned char tilt_servo_id;
long int tracking_period; long long int tracking_period;
short int tracking_period_ms; short int tracking_period_ms;
unsigned char tracking_enabled; unsigned char tracking_enabled;
}THeadMModule; }THeadMModule;
// public functions // 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); TMotionModule *head_tracking_get_module(THeadMModule *head);
void head_tracking_start(THeadMModule *joint); void head_tracking_start(THeadMModule *head);
void head_tracking_stop(THeadMModule *joint); void head_tracking_stop(THeadMModule *head);
unsigned char head_is_tracking(THeadMModule *joint); unsigned char head_is_tracking(THeadMModule *head);
void head_tracking_set_pan_range(THeadMModule *joint,short int max_angle,short int min_angle); void head_tracking_set_pan_range(THeadMModule *head,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_get_pan_range(THeadMModule *head,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_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 *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 *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 *joint,short int max_angle,short int min_angle); void head_tracking_set_tilt_range(THeadMModule *head,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_get_tilt_range(THeadMModule *head,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_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 *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 *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 *joint,short int target); void head_tracking_set_pan_pan(THeadMModule *head,short int target);
void head_tracking_set_tilt_target(THeadMModule *joint,short int target); void head_tracking_set_tilt_target(THeadMModule *head,short int target);
void head_tracking_set_pan_servo_id(THeadMModule *joint,unsigned char servo_id); void head_tracking_set_pan_servo_id(THeadMModule *head,unsigned char servo_id);
void head_tracking_set_tilt_servo_id(THeadMModule *joint,unsigned char servo_id); void head_tracking_set_tilt_servo_id(THeadMModule *head,unsigned char servo_id);
#ifdef __cplusplus #ifdef __cplusplus
} }
......
dynamixel_manager/src/modules/head_tracking.c
+ 18
− 19
View file @ 1a306833
...@@ -78,9 +78,9 @@ void head_tracking_write_cmd(THeadMModule *module,unsigned short int address,uns ...@@ -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)) 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); 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)) 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)) 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); head_tracking_set_pan_range(module,max,min);
} }
if(ram_in_window(module->ram_base_address+HEAD_TRACKING_MAX_TILT_OFFSET,4,address,length)) 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 ...@@ -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)) 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); 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)) 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)) 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); head_tracking_set_tilt_range(module,max,min);
} }
if(ram_in_window(module->ram_base_address+HEAD_TRACKING_PAN_TARGET_OFFSET,2,address,length)) 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 ...@@ -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)) 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); 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); module->tracking_pan.target_angle=(target<<9);
} }
...@@ -122,7 +122,7 @@ void head_tracking_write_cmd(THeadMModule *module,unsigned short int address,uns ...@@ -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)) 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); 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); module->tracking_tilt.target_angle=(target<<9);
} }
...@@ -151,24 +151,23 @@ void head_tracking_set_module(void *module,unsigned char servo_id) ...@@ -151,24 +151,23 @@ void head_tracking_set_module(void *module,unsigned char servo_id)
void head_tracking_pre_process(void *module) void head_tracking_pre_process(void *module)
{ {
THeadMModule *head=(THeadMModule *)module; THeadMModule *head=(THeadMModule *)module;
long int pan_error, tilt_error; signed long long int pan_error, tilt_error;
long int derivative_pan=0, derivative_tilt=0; signed long long int derivative_pan=0, derivative_tilt=0;
if(head->tracking_enabled) 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) 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; head->tracking_pan.integral_part+=(pan_error*head->tracking_period)>>16;
if(head->tracking_pan.integral_part>head->tracking_pan.integral_clamp) if(head->tracking_pan.integral_part>head->tracking_pan.integral_clamp)
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) else if(head->tracking_pan.integral_part<-head->tracking_pan.integral_clamp)
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; 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+=((signed long long int)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+=((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+=((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.kd*derivative_pan)>>16;
if(head->mmodule.manager->servo_values[head->pan_servo_id].target_angle>head->tracking_pan.max_angle) 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; 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) 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) ...@@ -177,16 +176,16 @@ void head_tracking_pre_process(void *module)
} }
if(mmanager_get_module(head->mmodule.manager,head->tilt_servo_id)==MM_HEAD) 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; head->tracking_tilt.integral_part+=(tilt_error*head->tracking_period)>>16;
if(head->tracking_tilt.integral_part>head->tracking_tilt.integral_clamp) if(head->tracking_tilt.integral_part>head->tracking_tilt.integral_clamp)
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) else if(head->tracking_tilt.integral_part<-head->tracking_tilt.integral_clamp)
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; 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+=((signed long long int)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+=((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+=((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.kd*derivative_tilt)>>16;
if(head->mmodule.manager->servo_values[head->tilt_servo_id].target_angle>head->tracking_tilt.max_angle) 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; 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) 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) ...@@ -239,8 +238,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_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_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