diff --git a/dynamixel_manager/src/dyn_manager.c b/dynamixel_manager/src/dyn_manager.c index db25cf8667d0a5688fcf111927b2c603f67bbc22..63e7ee57fa4f9b27a1d74c0edd06096231a62b16 100644 --- a/dynamixel_manager/src/dyn_manager.c +++ b/dynamixel_manager/src/dyn_manager.c @@ -351,6 +351,7 @@ void dyn_manager_scan(TDynManager *manager) unsigned short int model; manager->present_devices=0x00000000; + manager->num_devices=0; // scan all dynamixel buses for(i=0;i<manager->num_masters;i++) { @@ -939,7 +940,7 @@ void dyn_manager_loop(TDynManager *manager) TDynManagerSingleOp *single_op[DYN_MANAGER_MAX_NUM_MASTERS]; TDynManagerSyncOp *sync_op[DYN_MANAGER_MAX_NUM_MASTERS]; TDynManagerBulkOp *bulk_op[DYN_MANAGER_MAX_NUM_MASTERS]; - static unsigned char modules_period_count[DYN_MANAGER_MAX_NUM_MASTERS]={0}; + static unsigned char modules_period_count[DYN_MANAGER_MAX_NUM_MODULES]={0}; static unsigned char single_op_period_count[DYN_MANAGER_MAX_NUM_SINGLE_OP]={0}; static unsigned char sync_op_period_count[DYN_MANAGER_MAX_NUM_SINGLE_OP]={0}; static unsigned char bulk_op_period_count[DYN_MANAGER_MAX_NUM_SINGLE_OP]={0}; @@ -973,6 +974,7 @@ void dyn_manager_loop(TDynManager *manager) { if(manager->modules[i]->pre_process!=0x00000000) manager->modules[i]->pre_process(manager->modules[i]->data); + modules_period_count[i]=0; } } } diff --git a/dynamixel_manager/src/dyn_module.c b/dynamixel_manager/src/dyn_module.c index 342b4ac7c8c565035f38ff99743059651899b2fd..949fe66af2017bf01bc7ed029dfadd6953da41f3 100644 --- a/dynamixel_manager/src/dyn_module.c +++ b/dynamixel_manager/src/dyn_module.c @@ -30,7 +30,7 @@ unsigned char dyn_module_init(TDynModule *module,TMemory *memory,unsigned short module->assigned_ids[i]=0x00; module->num_assigned_ids=0x00; module->period_count=0x00; - module->enabled=0x00; + module->enabled=0x01; module->add_device=0x00000000; module->set_period=0x00000000; module->setup=0x00000000; diff --git a/dynamixel_manager/src/modules/motion_manager.c b/dynamixel_manager/src/modules/motion_manager.c index 84dcad807083e1c25ab7a892f25a54fb3aba71ae..3e5a676b5384bb71abd3ce4386a210ebcd3c3742 100644 --- a/dynamixel_manager/src/modules/motion_manager.c +++ b/dynamixel_manager/src/modules/motion_manager.c @@ -65,6 +65,7 @@ void mmanager_write_cmd(void *module,unsigned short int address,unsigned short i } } } + ram_write_table(manager->memory,address,length,data); } unsigned short int mmanager_angle_to_value(TMotionManager *mmanager,unsigned char servo_id,short int angle); @@ -171,7 +172,11 @@ void mmanager_setup(TMotionManager *mmanager) } } if(num>0) + { + if(mmanager->motion_op[0]!=0x00000000) + dyn_manager_delete_op(mmanager->dyn_module.manager,mmanager->motion_op[0]); mmanager->motion_op[0]=dyn_manager_sync_op_new(mmanager->dyn_module.manager,DYN_MANAGER_WRITE,num,ids,start_address,4,(unsigned char * const*)&data); + } num=0; for(i=0;i<DYN_MANAGER_MAX_NUM_DEVICES;i++) { @@ -191,7 +196,11 @@ void mmanager_setup(TMotionManager *mmanager) } } if(num>0) + { + if(mmanager->motion_op[1]!=0x00000000) + dyn_manager_delete_op(mmanager->dyn_module.manager,mmanager->motion_op[1]); mmanager->motion_op[1]=dyn_manager_sync_op_new(mmanager->dyn_module.manager,DYN_MANAGER_WRITE,num,ids,start_address,2,(unsigned char * const*)&data); + } /* create a feedback operation with the servos that support bulk read */ num=0; for(i=0;i<DYN_MANAGER_MAX_NUM_DEVICES;i++) @@ -209,7 +218,11 @@ void mmanager_setup(TMotionManager *mmanager) } } if(num>0) + { + if(mmanager->feedback_op[0]!=0x00000000) + dyn_manager_delete_op(mmanager->dyn_module.manager,mmanager->feedback_op[0]); mmanager->feedback_op[0]=dyn_manager_bulk_op_new(mmanager->dyn_module.manager,DYN_MANAGER_READ,num,ids,address,length,(unsigned char * const*)data); + } } void mmanager_set_period(TMotionManager *mmanager,unsigned short period_ms) diff --git a/dynamixel_manager/src/modules/motion_pages.c b/dynamixel_manager/src/modules/motion_pages.c index bf4f7d8a10551d3545493e0aa698a5088e136666..34bec5e5cb174d3825a84f1d76b7fcb25c88fad5 100755 --- a/dynamixel_manager/src/modules/motion_pages.c +++ b/dynamixel_manager/src/modules/motion_pages.c @@ -42,4 +42,4 @@ inline unsigned char pages_get_slope(TPage *page,unsigned char servo_id) return 0x01<<(page->header.slope[servo_id]&0x0F); } -TPage motion_pages[MAX_PAGES] __attribute__ ((section (".pages"))) __attribute__((weak)) ; +TPage motion_pages[MAX_PAGES] __attribute__ ((section (".pages"))) __attribute__((weak));