From e718a10940ce8e1690daebe05d1d87ec39c9021d Mon Sep 17 00:00:00 2001
From: Sergi Hernandez Juan <shernand@iri.upc.edu>
Date: Fri, 13 Mar 2020 19:29:59 +0100
Subject: [PATCH] num_assigned_ids is only incremented if the corresponding
device has not beeen initialized yet.
---
dynamixel_manager/src/dyn_manager.c | 4 ----
dynamixel_manager/src/dyn_module.c | 9 ++++++---
dynamixel_manager/src/modules/motion_manager.c | 4 ++--
3 files changed, 8 insertions(+), 9 deletions(-)
diff --git a/dynamixel_manager/src/dyn_manager.c b/dynamixel_manager/src/dyn_manager.c
index 59076d3..3685f03 100644
--- a/dynamixel_manager/src/dyn_manager.c
+++ b/dynamixel_manager/src/dyn_manager.c
@@ -1048,7 +1048,6 @@ void dyn_manager_loop(TDynManager *manager)
}
// send all the commands
- std::cout << "start loop" << std::endl;
dyn_manager_lock(manager);
do{
for(i=0;i<manager->num_masters;i++)
@@ -1109,7 +1108,6 @@ void dyn_manager_loop(TDynManager *manager)
dyn_master_start_sync_read(manager->masters[i],sync_op[i]->num_devices,sync_op[i]->ids,sync_op[i]->address,sync_op[i]->length,sync_op[i]->data);
else
dyn_master_start_sync_write(manager->masters[i],sync_op[i]->num_devices,sync_op[i]->ids,sync_op[i]->address,sync_op[i]->length,sync_op[i]->data);
- usleep(300);
states[i]=wait_sync_ops;
sync_op_period_count[current_sync_op[i]]=0;
}
@@ -1149,7 +1147,6 @@ void dyn_manager_loop(TDynManager *manager)
dyn_master_start_bulk_read(manager->masters[i],bulk_op[i]->num_devices,bulk_op[i]->ids,bulk_op[i]->address,bulk_op[i]->length,bulk_op[i]->data);
else
dyn_master_start_bulk_write(manager->masters[i],bulk_op[i]->num_devices,bulk_op[i]->ids,bulk_op[i]->address,bulk_op[i]->length,bulk_op[i]->data);
- usleep(300);
states[i]=wait_bulk_ops;
bulk_op_period_count[current_bulk_op[i]]=0;
}
@@ -1183,7 +1180,6 @@ void dyn_manager_loop(TDynManager *manager)
}
}while(loop_done>0);
dyn_manager_unlock(manager);
- std::cout << "loop done" << std::endl;
// execute the pre_process functions for all modules
for(i=0;i<manager->num_modules;i++)
diff --git a/dynamixel_manager/src/dyn_module.c b/dynamixel_manager/src/dyn_module.c
index 949fe66..5f52cb2 100644
--- a/dynamixel_manager/src/dyn_module.c
+++ b/dynamixel_manager/src/dyn_module.c
@@ -76,9 +76,12 @@ void dyn_module_add_device(TDynModule *module,unsigned char id, unsigned short i
for(i=0;i<module->num_models;i++)
{
if(model==module->models[i])
- {
- module->assigned_ids[id]=0x01;
- module->num_assigned_ids++;
+ {
+ if(module->assigned_ids[id]==0x00)
+ {
+ module->assigned_ids[id]=0x01;
+ module->num_assigned_ids++;
+ }
if(module->add_device!=0x00000000)
module->add_device(module->data,id,model);
break;
diff --git a/dynamixel_manager/src/modules/motion_manager.c b/dynamixel_manager/src/modules/motion_manager.c
index c542a68..29dc9a5 100644
--- a/dynamixel_manager/src/modules/motion_manager.c
+++ b/dynamixel_manager/src/modules/motion_manager.c
@@ -135,8 +135,8 @@ void mmanager_add_device(TMotionManager *mmanager,unsigned char id,unsigned shor
if(id<DYN_MANAGER_MAX_NUM_DEVICES)
{
for(i=0;i<NUM_SERVO_MODELS;i++)
- if(model==servo_data[i].model)
- mmanager->servo_configs[id]=&servo_data[i];
+ if(model==servo_data[i].model)
+ mmanager->servo_configs[id]=&servo_data[i];
mmanager_read_register(mmanager,id,&mmanager->servo_configs[id]->registers[current_pos],&mmanager->servo_values[id].current_value);
mmanager_read_register(mmanager,id,&mmanager->servo_configs[id]->registers[min_angle_limit],&cw_value);
--
GitLab