diff --git a/dynamixel_manager/src/dyn_manager.c b/dynamixel_manager/src/dyn_manager.c
index 59076d3b0623d4dd71bd1033c571b76a09604af0..3685f03401909efe5a4818760a8d8de87b91a1ec 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 949fe66af2017bf01bc7ed029dfadd6953da41f3..5f52cb2634ebe3cf9d955b2f85a43d1e2dd20544 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 c542a68b79322ca9970d1ea22e9df4ffdb71b81b..29dc9a569a13952abd8fd15bd6efdc79d1800f73 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);