diff --git a/dynamixel_manager/include/dyn_manager.h b/dynamixel_manager/include/dyn_manager.h
index 3069f47f30090c25f97f73240b36d2d247529d3a..ab8e01a12620396670419d9a382603cdd623defb 100644
--- a/dynamixel_manager/include/dyn_manager.h
+++ b/dynamixel_manager/include/dyn_manager.h
@@ -127,6 +127,10 @@ typedef struct{
unsigned char stop_flag;
unsigned int present_devices;
volatile unsigned char lock;
+ unsigned char modules_period_count[DYN_MANAGER_MAX_NUM_MODULES];
+ unsigned char single_op_period_count[DYN_MANAGER_MAX_NUM_SINGLE_OP];
+ unsigned char sync_op_period_count[DYN_MANAGER_MAX_NUM_SYNC_OP];
+ unsigned char bulk_op_period_count[DYN_MANAGER_MAX_NUM_BULK_OP];
}TDynManager;
// public functions
diff --git a/dynamixel_manager/src/dyn_manager.c b/dynamixel_manager/src/dyn_manager.c
index 3685f03401909efe5a4818760a8d8de87b91a1ec..b9282ff446e165f93de35bf79334bbae8d3aa43a 100644
--- a/dynamixel_manager/src/dyn_manager.c
+++ b/dynamixel_manager/src/dyn_manager.c
@@ -253,6 +253,7 @@ unsigned char dyn_manager_init(TDynManager *manager,TMemory *memory,TScheduler *
manager->operations[i].single_op[j].address=0x0000;
manager->operations[i].single_op[j].length=0x0000;
manager->operations[i].single_op[j].data=0x00000000;
+ manager->single_op_period_count[i]=0;
}
manager->operations[i].num_single_op=0;
for(j=0;j<DYN_MANAGER_MAX_NUM_SYNC_OP;j++)
@@ -269,6 +270,7 @@ unsigned char dyn_manager_init(TDynManager *manager,TMemory *memory,TScheduler *
}
manager->operations[i].sync_op[j].address=0x0000;
manager->operations[i].sync_op[j].length=0x0000;
+ manager->sync_op_period_count[i]=0;
}
manager->operations[i].num_sync_op=0;
for(j=0;j<DYN_MANAGER_MAX_NUM_BULK_OP;j++)
@@ -285,12 +287,16 @@ unsigned char dyn_manager_init(TDynManager *manager,TMemory *memory,TScheduler *
manager->operations[i].bulk_op[j].length[k]=0x0000;
manager->operations[i].bulk_op[j].data[k]=0x00000000;
}
+ manager->bulk_op_period_count[i]=0;
}
manager->operations[i].num_bulk_op=0;
}
manager->num_masters=0;
for(i=0;i<DYN_MANAGER_MAX_NUM_MODULES;i++)
+ {
manager->modules[i]=0x00000000;
+ manager->modules_period_count[i]=0;
+ }
manager->num_modules=0;
for(i=0;i<DYN_MANAGER_MAX_NUM_DEVICES;i++)
{
@@ -659,7 +665,7 @@ OP_HANDLE *dyn_manager_sync_op_new(TDynManager *manager,unsigned char mode,unsig
current_op->length=length;
current_op->ids[current_op->num_devices]=ids[i];
current_op->data[current_op->num_devices]=data[i];
- current_op->num_devices++;
+ current_op->num_devices=1;
manager->operations[master_index].num_sync_op++;
}
}
@@ -689,18 +695,21 @@ OP_HANDLE *dyn_manager_sync_op_new(TDynManager *manager,unsigned char mode,unsig
void dyn_manager_sync_op_add_devices(TDynManager *manager,OP_HANDLE *op,unsigned char num,unsigned char *ids,unsigned char * const data[])
{
- TDynManagerSyncOp *current_op;
- unsigned char i,j;
+ TDynManagerSyncOp *current_op,*other_op;
+ unsigned char i,j,master_index,current_op_index;
dyn_manager_lock(manager);
- for(i=0;i<manager->num_masters;i++)
+ for(i=0;i<num;i++)
{
- if(op->op_index[i]!=0xFF)
+ if((master_index=dyn_manager_check_id(manager,ids[i]))==0xFF)
+ continue;
+ else
{
- current_op=&manager->operations[i].sync_op[op->op_index[i]];
- for(j=0;j<num;j++)
+ if(op->op_index[master_index]!=0xFF)
{
- if(dyn_manager_id_present(current_op->num_devices,current_op->ids,ids[j])==0x00)
+ current_op_index=op->op_index[master_index];
+ current_op=&manager->operations[master_index].sync_op[current_op_index];
+ if(dyn_manager_id_present(current_op->num_devices,current_op->ids,ids[i])==0x00)
{
if(current_op->num_devices<DYN_MANAGER_MAX_NUM_DEVICES)
{
@@ -710,6 +719,30 @@ void dyn_manager_sync_op_add_devices(TDynManager *manager,OP_HANDLE *op,unsigned
}
}
}
+ else
+ {
+ current_op_index=manager->operations[master_index].num_sync_op;
+ if(current_op_index<DYN_MANAGER_MAX_NUM_SYNC_OP)
+ {
+ current_op=&manager->operations[master_index].sync_op[current_op_index];
+ for(j=0;j<DYN_MANAGER_MAX_NUM_MASTERS;j++)
+ if(op->op_index[j]!=0xFF)
+ {
+ other_op=&manager->operations[j].sync_op[current_op_index];
+ current_op->address=other_op->address;
+ current_op->length=other_op->length;
+ current_op->common.operation=other_op->common.operation;
+ current_op->common.repetitions=other_op->common.repetitions;
+ current_op->common.period_count=other_op->common.period_count;
+ current_op->common.op_handle=other_op->common.op_handle;
+ }
+ op->op_index[master_index]=current_op_index;
+ current_op->ids[current_op->num_devices]=ids[i];
+ current_op->data[current_op->num_devices]=data[i];
+ current_op->num_devices=1;
+ manager->operations[master_index].num_sync_op++;
+ }
+ }
}
}
dyn_manager_unlock(manager);
@@ -875,29 +908,54 @@ OP_HANDLE *dyn_manager_bulk_op_new(TDynManager *manager,unsigned char mode,unsig
void dyn_manager_bulk_op_add_devices(TDynManager *manager,OP_HANDLE *op,unsigned char num,unsigned char *ids,unsigned short int *address, unsigned short int *length,unsigned char * const data[])
{
- TDynManagerBulkOp *current_op;
- unsigned char i,j;
+ TDynManagerBulkOp *current_op,*other_op;
+ unsigned char i,j,master_index,current_op_index;
dyn_manager_lock(manager);
- for(i=0;i<manager->num_masters;i++)
+ for(i=0;i<num;i++)
{
- if(op->op_index[i]!=0xFF)
+ if((master_index=dyn_manager_check_id(manager,ids[i]))==0xFF)
+ continue;
+ else
{
- current_op=&manager->operations[i].bulk_op[op->op_index[i]];
- for(j=0;j<num;j++)
+ if(op->op_index[master_index]!=0xFF)
{
- if(dyn_manager_id_present(current_op->num_devices,current_op->ids,ids[j])==0x00)
+ current_op_index=op->op_index[master_index];
+ current_op=&manager->operations[master_index].bulk_op[current_op_index];
+ if(dyn_manager_id_present(current_op->num_devices,current_op->ids,ids[i])==0x00)
{
if(current_op->num_devices<DYN_MANAGER_MAX_NUM_DEVICES)
{
current_op->ids[current_op->num_devices]=ids[i];
- current_op->address[current_op->num_devices]=address[i];
- current_op->length[current_op->num_devices]=length[i];
current_op->data[current_op->num_devices]=data[i];
current_op->num_devices++;
}
}
}
+ else
+ {
+ current_op_index=manager->operations[master_index].num_bulk_op;
+ if(current_op_index<DYN_MANAGER_MAX_NUM_SYNC_OP)
+ {
+ current_op=&manager->operations[master_index].bulk_op[current_op_index];
+ for(j=0;j<DYN_MANAGER_MAX_NUM_MASTERS;j++)
+ if(op->op_index[j]!=0xFF)
+ {
+ other_op=&manager->operations[j].bulk_op[current_op_index];
+ current_op->common.operation=other_op->common.operation;
+ current_op->common.repetitions=other_op->common.repetitions;
+ current_op->common.period_count=other_op->common.period_count;
+ current_op->common.op_handle=other_op->common.op_handle;
+ }
+ op->op_index[master_index]=current_op_index;
+ current_op->ids[current_op->num_devices]=ids[i];
+ current_op->data[current_op->num_devices]=data[i];
+ current_op->address[current_op->num_devices]=address[i];
+ current_op->length[current_op->num_devices]=length[i];
+ current_op->num_devices=1;
+ manager->operations[master_index].num_bulk_op++;
+ }
+ }
}
}
dyn_manager_unlock(manager);
@@ -1018,12 +1076,9 @@ 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_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};
// initialize the remaining operations
+ dyn_manager_lock(manager);
for(i=0;i<manager->num_masters;i++)
{
states[i]=start_single_ops;
@@ -1037,18 +1092,17 @@ void dyn_manager_loop(TDynManager *manager)
{
if(manager->modules[i]->enabled)
{
- modules_period_count[i]++;
- if(modules_period_count[i]==manager->modules[i]->period_count)
+ manager->modules_period_count[i]++;
+ if(manager->modules_period_count[i]==manager->modules[i]->period_count)
{
if(manager->modules[i]->pre_process!=0x00000000)
manager->modules[i]->pre_process(manager->modules[i]->data);
- modules_period_count[i]=0;
+ manager->modules_period_count[i]=0;
}
}
}
// send all the commands
- dyn_manager_lock(manager);
do{
for(i=0;i<manager->num_masters;i++)
{
@@ -1058,16 +1112,16 @@ void dyn_manager_loop(TDynManager *manager)
states[i]=start_bulk_ops;
else
{
- single_op_period_count[current_single_op[i]]++;
+ manager->single_op_period_count[current_single_op[i]]++;
single_op[i]=&manager->operations[i].single_op[current_single_op[i]];
- if(single_op_period_count[current_single_op[i]]==single_op[i]->common.period_count)
+ if(manager->single_op_period_count[current_single_op[i]]==single_op[i]->common.period_count)
{
if((single_op[i]->common.operation&DYN_MANAGER_MODE_MASK)==DYN_MANAGER_READ)
dyn_master_start_read_table(manager->masters[i],single_op[i]->id,single_op[i]->address,single_op[i]->length,single_op[i]->data);
else
dyn_master_start_write_table(manager->masters[i],single_op[i]->id,single_op[i]->address,single_op[i]->length,single_op[i]->data);
states[i]=wait_single_ops;
- single_op_period_count[current_single_op[i]]=0;
+ manager->single_op_period_count[current_single_op[i]]=0;
}
else
{
@@ -1100,16 +1154,16 @@ void dyn_manager_loop(TDynManager *manager)
}
else
{
- sync_op_period_count[current_sync_op[i]]++;
+ manager->sync_op_period_count[current_sync_op[i]]++;
sync_op[i]=&manager->operations[i].sync_op[current_sync_op[i]];
- if(sync_op_period_count[current_sync_op[i]]==sync_op[i]->common.period_count)
+ if(manager->sync_op_period_count[current_sync_op[i]]==sync_op[i]->common.period_count)
{
if((sync_op[i]->common.operation&DYN_MANAGER_MODE_MASK)==DYN_MANAGER_READ)
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);
states[i]=wait_sync_ops;
- sync_op_period_count[current_sync_op[i]]=0;
+ manager->sync_op_period_count[current_sync_op[i]]=0;
}
else
{
@@ -1139,16 +1193,16 @@ void dyn_manager_loop(TDynManager *manager)
states[i]=start_sync_ops;
else
{
- bulk_op_period_count[current_bulk_op[i]]++;
+ manager->bulk_op_period_count[current_bulk_op[i]]++;
bulk_op[i]=&manager->operations[i].bulk_op[current_bulk_op[i]];
- if(bulk_op_period_count[current_bulk_op[i]]==bulk_op[i]->common.period_count)
+ if(manager->bulk_op_period_count[current_bulk_op[i]]==bulk_op[i]->common.period_count)
{
if((bulk_op[i]->common.operation&DYN_MANAGER_MODE_MASK)==DYN_MANAGER_READ)
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);
states[i]=wait_bulk_ops;
- bulk_op_period_count[current_bulk_op[i]]=0;
+ manager->bulk_op_period_count[current_bulk_op[i]]=0;
}
else
{
@@ -1179,21 +1233,21 @@ void dyn_manager_loop(TDynManager *manager)
}
}
}while(loop_done>0);
- dyn_manager_unlock(manager);
// execute the pre_process functions for all modules
for(i=0;i<manager->num_modules;i++)
{
if(manager->modules[i]->enabled)
{
- if(modules_period_count[i]==manager->modules[i]->period_count)
+ if(manager->modules_period_count[i]==manager->modules[i]->period_count)
{
if(manager->modules[i]->post_process!=0x00000000)
manager->modules[i]->post_process(manager->modules[i]->data);
- modules_period_count[i]=0;
+ manager->modules_period_count[i]=0;
}
}
}
+ dyn_manager_unlock(manager);
if(manager->stop_flag==0x01)
{
manager->running=0x00;
diff --git a/dynamixel_manager/src/modules/motion_manager.c b/dynamixel_manager/src/modules/motion_manager.c
index 29dc9a569a13952abd8fd15bd6efdc79d1800f73..cfc7a2d0f697c4a92de0c327a1d9b39937b6ec0a 100644
--- a/dynamixel_manager/src/modules/motion_manager.c
+++ b/dynamixel_manager/src/modules/motion_manager.c
@@ -366,17 +366,20 @@ void mmanager_enable_servo(TMotionManager *mmanager,unsigned char servo_id)
if(servo_id<DYN_MANAGER_MAX_NUM_DEVICES)
{
- mmanager->servo_values[servo_id].enabled=0x01;
- /* add an operation to enable the servo */
- if(mmanager->servo_configs[servo_id]!=0x00000000)
+ if(mmanager->servo_values[servo_id].enabled==0x00)
{
- if(mmanager->enable_op==0x00000000 || mmanager->enable_op->op_type==no_op)
+ mmanager->servo_values[servo_id].enabled=0x01;
+ /* add an operation to enable the servo */
+ if(mmanager->servo_configs[servo_id]!=0x00000000)
{
- mmanager->enable_op=dyn_manager_sync_op_new(mmanager->dyn_module.manager,DYN_MANAGER_WRITE,1,&servo_id,mmanager->servo_configs[servo_id]->registers[torque_en].address,1,&data);
- dyn_manager_set_op_repetitions(mmanager->dyn_module.manager,mmanager->enable_op,1);
+ if(mmanager->enable_op==0x00000000 || mmanager->enable_op->op_type==no_op)
+ {
+ mmanager->enable_op=dyn_manager_sync_op_new(mmanager->dyn_module.manager,DYN_MANAGER_WRITE,1,&servo_id,mmanager->servo_configs[servo_id]->registers[torque_en].address,1,&data);
+ dyn_manager_set_op_repetitions(mmanager->dyn_module.manager,mmanager->enable_op,1);
+ }
+ else
+ dyn_manager_sync_op_add_devices(mmanager->dyn_module.manager,mmanager->enable_op,1,&servo_id,&data);
}
- else
- dyn_manager_sync_op_add_devices(mmanager->dyn_module.manager,mmanager->enable_op,1,&servo_id,&data);
}
}
}
@@ -387,17 +390,20 @@ void mmanager_disable_servo(TMotionManager *mmanager,unsigned char servo_id)
if(servo_id<DYN_MANAGER_MAX_NUM_DEVICES)
{
- mmanager->servo_values[servo_id].enabled=0x00;
- /* add an operation to enable the servo */
- if(mmanager->servo_configs[servo_id]!=0x00000000)
+ if(mmanager->servo_values[servo_id].enabled==0x01)
{
- if(mmanager->enable_op==0x00000000 || mmanager->enable_op->op_type==no_op)
+ mmanager->servo_values[servo_id].enabled=0x00;
+ /* add an operation to enable the servo */
+ if(mmanager->servo_configs[servo_id]!=0x00000000)
{
- mmanager->enable_op=dyn_manager_sync_op_new(mmanager->dyn_module.manager,DYN_MANAGER_WRITE,1,&servo_id,mmanager->servo_configs[servo_id]->registers[torque_en].address,1,&data);
- dyn_manager_set_op_repetitions(mmanager->dyn_module.manager,mmanager->enable_op,1);
- }
- else
- dyn_manager_sync_op_add_devices(mmanager->dyn_module.manager,mmanager->enable_op,1,&servo_id,&data);
+ if(mmanager->enable_op==0x00000000 || mmanager->enable_op->op_type==no_op)
+ {
+ mmanager->enable_op=dyn_manager_sync_op_new(mmanager->dyn_module.manager,DYN_MANAGER_WRITE,1,&servo_id,mmanager->servo_configs[servo_id]->registers[torque_en].address,1,&data);
+ dyn_manager_set_op_repetitions(mmanager->dyn_module.manager,mmanager->enable_op,1);
+ }
+ else
+ dyn_manager_sync_op_add_devices(mmanager->dyn_module.manager,mmanager->enable_op,1,&servo_id,&data);
+ }
}
}
}