diff --git a/dynamixel_manager/src/dyn_manager.c b/dynamixel_manager/src/dyn_manager.c index ebf5509a09e996ea63331a7c0e8f3c1f55025e8b..59076d3b0623d4dd71bd1033c571b76a09604af0 100644 --- a/dynamixel_manager/src/dyn_manager.c +++ b/dynamixel_manager/src/dyn_manager.c @@ -394,7 +394,6 @@ void dyn_manager_scan(TDynManager *manager) } } manager->memory->data[manager->ram_base_address+DYN_MANAGER_NUM_DEVICES_OFFSET]=manager->num_devices; - manager->memory->data[manager->ram_base_address+DYN_MANAGER_CONTROL_OFFSET]&=(~DYN_MANAGER_SCANNING); manager->memory->data[manager->ram_base_address+DYN_MANAGER_PRESENT_DEVICES]=manager->present_devices&0x000000FF; manager->memory->data[manager->ram_base_address+DYN_MANAGER_PRESENT_DEVICES+1]=(manager->present_devices>>8)&0x000000FF; manager->memory->data[manager->ram_base_address+DYN_MANAGER_PRESENT_DEVICES+2]=(manager->present_devices>>16)&0x000000FF; @@ -1049,6 +1048,7 @@ 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,6 +1109,7 @@ 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; } @@ -1148,6 +1149,7 @@ 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; } @@ -1165,7 +1167,7 @@ void dyn_manager_loop(TDynManager *manager) if(done==DYN_BUSY) states[i]=wait_bulk_ops; else - { + { if(bulk_op[i]->common.repetitions>0 && bulk_op[i]->common.repetitions!=0xFF) bulk_op[i]->common.repetitions--; if(bulk_op[i]->common.repetitions==0)// remove operation from list @@ -1178,9 +1180,10 @@ void dyn_manager_loop(TDynManager *manager) case ops_done: states[i]=ops_done; break; } - } + } }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++)