From bb00112f48a65d63d5d2a12e7a773a5faa993969 Mon Sep 17 00:00:00 2001
From: Sergi Hernandez Juan <shernand@iri.upc.edu>
Date: Fri, 6 Mar 2020 16:44:27 +0100
Subject: [PATCH] Solved some minor bugs of the head tracking module.
---
dynamixel_manager/src/modules/head_tracking.c | 24 +++++++++----------
1 file changed, 12 insertions(+), 12 deletions(-)
diff --git a/dynamixel_manager/src/modules/head_tracking.c b/dynamixel_manager/src/modules/head_tracking.c
index 72e7861..7612930 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)
--
GitLab