diff --git a/dynamixel_manager/include/dyn_manager.h b/dynamixel_manager/include/dyn_manager.h index 0b8635b9f447022158f8124ca54b6c910c9fb954..2860e0ceb0d8171eac3301e512d87df7c28aca57 100644 --- a/dynamixel_manager/include/dyn_manager.h +++ b/dynamixel_manager/include/dyn_manager.h @@ -119,16 +119,17 @@ typedef struct{ unsigned char num_ops; unsigned char period_ms; TScheduler *scheduler; - sched_channel_t sch_channel; + sched_channel_t sch_loop_ch; + sched_channel_t sch_scan_ch; TMemory *memory; TMemModule mem_module; unsigned char running; - unsigned char do_scan; unsigned int present_devices; + volatile unsigned char lock; }TDynManager; // public functions -unsigned char dyn_manager_init(TDynManager *manager,TMemory *memory,TScheduler *scheduler,sched_channel_t sch_channel,unsigned short int eeprom_base_address,unsigned short int ram_base_address); +unsigned char dyn_manager_init(TDynManager *manager,TMemory *memory,TScheduler *scheduler,sched_channel_t loop_ch,sched_channel_t scan_ch,unsigned short int eeprom_base_address,unsigned short int ram_base_address); void dyn_manager_set_period(TDynManager *manager,unsigned char period_ms); static inline unsigned char dyn_manager_get_period(TDynManager *manager) { diff --git a/dynamixel_manager/src/dyn_manager.c b/dynamixel_manager/src/dyn_manager.c index 63e7ee57fa4f9b27a1d74c0edd06096231a62b16..3e70b32b8c366ee66e1ec271ee40da58c3c4ebe9 100644 --- a/dynamixel_manager/src/dyn_manager.c +++ b/dynamixel_manager/src/dyn_manager.c @@ -18,12 +18,22 @@ extern unsigned char dyn_master_is_bulk_read_done(TDynamixelMaster *master,unsig typedef enum {start_single_ops,wait_single_ops,start_sync_ops,wait_sync_ops,start_bulk_ops,wait_bulk_ops,ops_done} dyn_manager_states_t; // private functions +void dyn_manager_lock(TDynManager *manager) +{ + while(manager->lock==1); + manager->lock=1; +} + +void dyn_manager_unlock(TDynManager *manager) +{ + manager->lock=0; +} + void dyn_manager_start_scan(TDynManager *manager) { - if(manager->running==0x00) - dyn_manager_start(manager); - manager->do_scan=0x01; + dyn_manager_stop(manager); manager->memory->data[manager->ram_base_address+DYN_MANAGER_CONTROL_OFFSET]|=DYN_MANAGER_SCANNING; + scheduler_enable_channel(manager->scheduler,manager->sch_scan_ch); } void dyn_manager_write_cmd(TDynManager *module,unsigned short int address,unsigned short int length,unsigned char *data) @@ -73,7 +83,8 @@ TDynManagerOpCommon *dyn_manager_get_op_common(TDynManager *manager,unsigned cha default: return 0x00000000; } } - else return 0x00000000; + else + return 0x00000000; } OP_HANDLE *dyn_manager_get_free_op_handle(TDynManager *manager) @@ -137,7 +148,7 @@ void dyn_manager_delete_op_single_master(TDynManager *manager,unsigned char mast break; case DYN_MANAGER_SYNC: if(op->op_index[master_index]<(DYN_MANAGER_MAX_NUM_SYNC_OP-1)) { - for(j=op->op_index[master_index];j<manager->operations[master_index].num_sync_op;j++) + for(j=op->op_index[master_index];j<=manager->operations[master_index].num_sync_op;j++) { manager->operations[master_index].sync_op[j].common.operation=manager->operations[master_index].sync_op[j+1].common.operation; manager->operations[master_index].sync_op[j].common.repetitions=manager->operations[master_index].sync_op[j+1].common.repetitions; @@ -145,12 +156,12 @@ void dyn_manager_delete_op_single_master(TDynManager *manager,unsigned char mast manager->operations[master_index].sync_op[j].common.op_handle=manager->operations[master_index].sync_op[j+1].common.op_handle; manager->operations[master_index].sync_op[j].address=manager->operations[master_index].sync_op[j+1].address; manager->operations[master_index].sync_op[j].length=manager->operations[master_index].sync_op[j+1].length; - manager->operations[master_index].sync_op[j].num_devices=manager->operations[master_index].sync_op[j+1].num_devices; for(k=0;k<manager->operations[master_index].sync_op[j].num_devices;k++) { manager->operations[master_index].sync_op[j].ids[k]=manager->operations[master_index].sync_op[j+1].ids[k]; manager->operations[master_index].sync_op[j].data[k]=manager->operations[master_index].sync_op[j+1].data[k]; } + manager->operations[master_index].sync_op[j].num_devices=manager->operations[master_index].sync_op[j+1].num_devices; } } else @@ -161,12 +172,12 @@ void dyn_manager_delete_op_single_master(TDynManager *manager,unsigned char mast manager->operations[master_index].sync_op[op->op_index[master_index]].common.op_handle=0x00000000; manager->operations[master_index].sync_op[op->op_index[master_index]].address=0x0000; manager->operations[master_index].sync_op[op->op_index[master_index]].length=0x0000; - manager->operations[master_index].sync_op[op->op_index[master_index]].num_devices=0x00; for(k=0;k<manager->operations[master_index].sync_op[op->op_index[master_index]].num_devices;k++) { manager->operations[master_index].sync_op[op->op_index[master_index]].ids[k]=0xFF; manager->operations[master_index].sync_op[op->op_index[master_index]].data[k]=0x00000000; } + manager->operations[master_index].sync_op[op->op_index[master_index]].num_devices=0x00; } manager->operations[master_index].num_sync_op--; break; @@ -178,7 +189,6 @@ void dyn_manager_delete_op_single_master(TDynManager *manager,unsigned char mast manager->operations[master_index].bulk_op[j].common.repetitions=manager->operations[master_index].bulk_op[j+1].common.repetitions; manager->operations[master_index].bulk_op[j].common.period_count=manager->operations[master_index].bulk_op[j+1].common.period_count; manager->operations[master_index].bulk_op[j].common.op_handle=manager->operations[master_index].bulk_op[j+1].common.op_handle; - manager->operations[master_index].bulk_op[j].num_devices=manager->operations[master_index].bulk_op[j+1].num_devices; for(k=0;k<manager->operations[master_index].bulk_op[j].num_devices;k++) { manager->operations[master_index].bulk_op[j].address[k]=manager->operations[master_index].bulk_op[j+1].address[k]; @@ -186,6 +196,7 @@ void dyn_manager_delete_op_single_master(TDynManager *manager,unsigned char mast manager->operations[master_index].bulk_op[j].ids[k]=manager->operations[master_index].bulk_op[j+1].ids[k]; manager->operations[master_index].bulk_op[j].data[k]=manager->operations[master_index].bulk_op[j+1].data[k]; } + manager->operations[master_index].bulk_op[j].num_devices=manager->operations[master_index].bulk_op[j+1].num_devices; } } else @@ -194,7 +205,6 @@ void dyn_manager_delete_op_single_master(TDynManager *manager,unsigned char mast manager->operations[master_index].bulk_op[op->op_index[master_index]].common.repetitions=0x01; manager->operations[master_index].bulk_op[op->op_index[master_index]].common.period_count=0x01; manager->operations[master_index].bulk_op[op->op_index[master_index]].common.op_handle=0x00000000; - manager->operations[master_index].bulk_op[op->op_index[master_index]].num_devices=0x00; for(k=0;k<manager->operations[master_index].bulk_op[op->op_index[master_index]].num_devices;k++) { manager->operations[master_index].bulk_op[op->op_index[master_index]].address[k]=0x0000; @@ -202,6 +212,7 @@ void dyn_manager_delete_op_single_master(TDynManager *manager,unsigned char mast manager->operations[master_index].bulk_op[op->op_index[master_index]].ids[k]=0xFF; manager->operations[master_index].bulk_op[op->op_index[master_index]].data[k]=0x00000000; } + manager->operations[master_index].bulk_op[op->op_index[master_index]].num_devices=0x00; } manager->operations[master_index].num_bulk_op--; break; @@ -222,7 +233,7 @@ void dyn_manager_delete_op_single_master(TDynManager *manager,unsigned char mast } // public functions -unsigned char dyn_manager_init(TDynManager *manager,TMemory *memory,TScheduler *scheduler,sched_channel_t sch_channel,unsigned short int ram_base_address,unsigned short int eeprom_base_address) +unsigned char dyn_manager_init(TDynManager *manager,TMemory *memory,TScheduler *scheduler,sched_channel_t loop_ch,sched_channel_t scan_ch,unsigned short int ram_base_address,unsigned short int eeprom_base_address) { unsigned char i,j,k; @@ -293,10 +304,13 @@ unsigned char dyn_manager_init(TDynManager *manager,TMemory *memory,TScheduler * } manager->num_ops=0x00; manager->present_devices=0x00000000; + manager->lock=0; /* initialize scheduler */ - scheduler_set_channel(scheduler,sch_channel,(void(*)(void *))dyn_manager_loop,100,manager); + scheduler_set_channel(scheduler,loop_ch,(void(*)(void *))dyn_manager_loop,100,manager); + scheduler_set_channel_one_shot(scheduler,scan_ch,(void(*)(void *))dyn_manager_scan,100,manager); manager->scheduler=scheduler; - manager->sch_channel=sch_channel; + manager->sch_loop_ch=loop_ch; + manager->sch_scan_ch=scan_ch; /* initialize memory module */ mem_module_init(&manager->mem_module); @@ -313,7 +327,6 @@ unsigned char dyn_manager_init(TDynManager *manager,TMemory *memory,TScheduler * manager->ram_base_address=ram_base_address; manager->memory=memory; manager->running=0x00; - manager->do_scan=0x00; return 0x01; } @@ -322,7 +335,7 @@ void dyn_manager_set_period(TDynManager *manager,unsigned char period_ms) { unsigned char i; - scheduler_change_period(manager->scheduler,manager->sch_channel,period_ms); + scheduler_change_period(manager->scheduler,manager->sch_loop_ch,period_ms); for(i=0;i<DYN_MANAGER_MAX_NUM_MODULES;i++) if(manager->modules[i]!=0x00000000 && manager->modules[i]->set_period!=0x00000000) manager->modules[i]->set_period(manager->modules[i]->data,period_ms*manager->modules[i]->period_count); @@ -332,14 +345,14 @@ void dyn_manager_set_period(TDynManager *manager,unsigned char period_ms) void dyn_manager_start(TDynManager *manager) { - scheduler_enable_channel(manager->scheduler,manager->sch_channel); + scheduler_enable_channel(manager->scheduler,manager->sch_loop_ch); manager->running=0x01; manager->memory->data[manager->ram_base_address+DYN_MANAGER_CONTROL_OFFSET]|=DYN_MANAGER_RUNNING; } void dyn_manager_stop(TDynManager *manager) { - scheduler_disable_channel(manager->scheduler,manager->sch_channel); + scheduler_disable_channel(manager->scheduler,manager->sch_loop_ch); manager->running=0x00; manager->memory->data[manager->ram_base_address+DYN_MANAGER_CONTROL_OFFSET]&=(~DYN_MANAGER_RUNNING); } @@ -386,6 +399,7 @@ void dyn_manager_scan(TDynManager *manager) for(i=0;i<manager->num_modules;i++) if(manager->modules[i]->setup!=0x00000000) manager->modules[i]->setup(manager->modules[i]->data); + manager->memory->data[manager->ram_base_address+DYN_MANAGER_CONTROL_OFFSET]&=(~DYN_MANAGER_SCANNING); } void dyn_manager_add_master(TDynManager *manager,TDynamixelMaster *master) @@ -504,14 +518,24 @@ OP_HANDLE *dyn_manager_single_op_new(TDynManager *manager,unsigned char mode,uns TDynManagerSingleOp *current_op; OP_HANDLE *op_handle; + dyn_manager_lock(manager); if((master_index=dyn_manager_check_id(manager,id))==0xFF) + { + dyn_manager_unlock(manager); return 0x00000000; + } if((op_handle=dyn_manager_get_free_op_handle(manager))==0x00000000) + { + dyn_manager_unlock(manager); return 0x00000000; + } if(mode==DYN_MANAGER_READ || mode==DYN_MANAGER_WRITE) op_handle->op_type=(op_type_t)(DYN_MANAGER_SINGLE|mode); else + { + dyn_manager_unlock(manager); return 0x00000000; + } current_op_index=manager->operations[master_index].num_single_op; if(current_op_index<DYN_MANAGER_MAX_NUM_SINGLE_OP) { @@ -529,8 +553,11 @@ OP_HANDLE *dyn_manager_single_op_new(TDynManager *manager,unsigned char mode,uns else { op_handle->op_type=no_op; + dyn_manager_unlock(manager); return 0x00000000; } + dyn_manager_unlock(manager); + return op_handle; } @@ -538,44 +565,52 @@ void dyn_manager_single_op_change_id(TDynManager *manager,OP_HANDLE *op,unsigned { unsigned char i; + dyn_manager_lock(manager); for(i=0;i<manager->num_masters;i++) { if(op->op_index[i]!=0xFF) manager->operations[i].single_op[op->op_index[i]].id=id; } + dyn_manager_unlock(manager); } void dyn_manager_single_op_change_address(TDynManager *manager,OP_HANDLE *op,unsigned short int address) { unsigned char i; + dyn_manager_lock(manager); for(i=0;i<manager->num_masters;i++) { if(op->op_index[i]!=0xFF) manager->operations[i].single_op[op->op_index[i]].address=address; } + dyn_manager_unlock(manager); } void dyn_manager_single_op_change_length(TDynManager *manager,OP_HANDLE *op,unsigned short int length) { unsigned char i; + dyn_manager_lock(manager); for(i=0;i<manager->num_masters;i++) { if(op->op_index[i]!=0xFF) manager->operations[i].single_op[op->op_index[i]].length=length; } + dyn_manager_unlock(manager); } void dyn_manager_single_op_change_data(TDynManager *manager,OP_HANDLE *op,unsigned char *data) { unsigned char i; + dyn_manager_lock(manager); for(i=0;i<manager->num_masters;i++) { if(op->op_index[i]!=0xFF) manager->operations[i].single_op[op->op_index[i]].data=data; } + dyn_manager_unlock(manager); } // sync operation functions @@ -585,17 +620,25 @@ OP_HANDLE *dyn_manager_sync_op_new(TDynManager *manager,unsigned char mode,unsig TDynManagerSyncOp *current_op; OP_HANDLE *op_handle; + dyn_manager_lock(manager); if((op_handle=dyn_manager_get_free_op_handle(manager))==0x00000000) + { + dyn_manager_unlock(manager); return 0x00000000; + } if(mode==DYN_MANAGER_READ || mode==DYN_MANAGER_WRITE) op_handle->op_type=(op_type_t)(DYN_MANAGER_SYNC|mode); else + { + dyn_manager_unlock(manager); return 0x00000000; + } for(i=0;i<num;i++) { if((master_index=dyn_manager_check_id(manager,ids[i]))==0xFF) { op_handle->op_type=no_op; + dyn_manager_unlock(manager); return 0x00000000; } if(op_handle->op_index[master_index]==0xFF) @@ -620,6 +663,7 @@ OP_HANDLE *dyn_manager_sync_op_new(TDynManager *manager,unsigned char mode,unsig else { op_handle->op_type=no_op; + dyn_manager_unlock(manager); return 0x00000000; } } @@ -636,6 +680,7 @@ OP_HANDLE *dyn_manager_sync_op_new(TDynManager *manager,unsigned char mode,unsig } } manager->num_ops++; + dyn_manager_unlock(manager); return op_handle; } @@ -644,6 +689,7 @@ void dyn_manager_sync_op_add_devices(TDynManager *manager,OP_HANDLE *op,unsigned TDynManagerSyncOp *current_op; unsigned char i,j; + dyn_manager_lock(manager); for(i=0;i<manager->num_masters;i++) { if(op->op_index[i]!=0xFF) @@ -663,6 +709,7 @@ void dyn_manager_sync_op_add_devices(TDynManager *manager,OP_HANDLE *op,unsigned } } } + dyn_manager_unlock(manager); } void dyn_manager_sync_op_remove_devices(TDynManager *manager,OP_HANDLE *op,unsigned char num,unsigned char *ids) @@ -670,6 +717,7 @@ void dyn_manager_sync_op_remove_devices(TDynManager *manager,OP_HANDLE *op,unsig TDynManagerSyncOp *current_op; unsigned char i,j,k,device_index; + dyn_manager_lock(manager); for(i=0;i<manager->num_masters;i++) { if(op->op_index[i]!=0xFF) @@ -699,28 +747,33 @@ void dyn_manager_sync_op_remove_devices(TDynManager *manager,OP_HANDLE *op,unsig dyn_manager_delete_op_single_master(manager,i,op); } } + dyn_manager_unlock(manager); } void dyn_manager_sync_op_change_address(TDynManager *manager,OP_HANDLE *op,unsigned short int address) { unsigned char i; + dyn_manager_lock(manager); for(i=0;i<manager->num_masters;i++) { if(op->op_index[i]!=0xFF) manager->operations[i].sync_op[op->op_index[i]].address=address; } + dyn_manager_unlock(manager); } void dyn_manager_sync_op_change_length(TDynManager *manager,OP_HANDLE *op,unsigned short int length) { unsigned char i; + dyn_manager_lock(manager); for(i=0;i<manager->num_masters;i++) { if(op->op_index[i]!=0xFF) manager->operations[i].sync_op[op->op_index[i]].length=length; } + dyn_manager_unlock(manager); } void dyn_manager_sync_op_change_data(TDynManager *manager,OP_HANDLE *op,unsigned char num,unsigned char *ids,unsigned char * const data[]) @@ -728,6 +781,7 @@ void dyn_manager_sync_op_change_data(TDynManager *manager,OP_HANDLE *op,unsigned TDynManagerSyncOp *current_op; unsigned char i,j,device_index; + dyn_manager_lock(manager); for(i=0;i<manager->num_masters;i++) { if(op->op_index[i]!=0xFF) @@ -740,6 +794,7 @@ void dyn_manager_sync_op_change_data(TDynManager *manager,OP_HANDLE *op,unsigned } } } + dyn_manager_unlock(manager); } // bulk operation functions @@ -749,17 +804,25 @@ OP_HANDLE *dyn_manager_bulk_op_new(TDynManager *manager,unsigned char mode,unsig TDynManagerBulkOp *current_op; OP_HANDLE *op_handle; + dyn_manager_lock(manager); if((op_handle=dyn_manager_get_free_op_handle(manager))==0x00000000) + { + dyn_manager_unlock(manager); return 0x00000000; + } if(mode==DYN_MANAGER_READ || mode==DYN_MANAGER_WRITE) op_handle->op_type=(op_type_t)(DYN_MANAGER_BULK|mode); else + { + dyn_manager_unlock(manager); return 0x00000000; + } for(i=0;i<num;i++) { if((master_index=dyn_manager_check_id(manager,ids[i]))==0xFF) { op_handle->op_type=no_op; + dyn_manager_unlock(manager); return 0x00000000; } if(op_handle->op_index[master_index]==0xFF) @@ -784,6 +847,7 @@ OP_HANDLE *dyn_manager_bulk_op_new(TDynManager *manager,unsigned char mode,unsig else { op_handle->op_type=no_op; + dyn_manager_unlock(manager); return 0x00000000; } } @@ -802,6 +866,7 @@ OP_HANDLE *dyn_manager_bulk_op_new(TDynManager *manager,unsigned char mode,unsig } } manager->num_ops++; + dyn_manager_unlock(manager); return op_handle; } @@ -810,6 +875,7 @@ void dyn_manager_bulk_op_add_devices(TDynManager *manager,OP_HANDLE *op,unsigned TDynManagerBulkOp *current_op; unsigned char i,j; + dyn_manager_lock(manager); for(i=0;i<manager->num_masters;i++) { if(op->op_index[i]!=0xFF) @@ -831,6 +897,7 @@ void dyn_manager_bulk_op_add_devices(TDynManager *manager,OP_HANDLE *op,unsigned } } } + dyn_manager_unlock(manager); } void dyn_manager_bulk_op_remove_devices(TDynManager *manager,OP_HANDLE *op,unsigned char num,unsigned char *ids) @@ -838,6 +905,7 @@ void dyn_manager_bulk_op_remove_devices(TDynManager *manager,OP_HANDLE *op,unsig TDynManagerBulkOp *current_op; unsigned char i,j,k,device_index; + dyn_manager_lock(manager); for(i=0;i<manager->num_masters;i++) { if(op->op_index[i]!=0xFF) @@ -871,6 +939,7 @@ void dyn_manager_bulk_op_remove_devices(TDynManager *manager,OP_HANDLE *op,unsig dyn_manager_delete_op_single_master(manager,i,op); } } + dyn_manager_unlock(manager); } void dyn_manager_bulk_op_change_address(TDynManager *manager,OP_HANDLE *op,unsigned char num,unsigned char *ids,unsigned short int *address) @@ -878,6 +947,7 @@ void dyn_manager_bulk_op_change_address(TDynManager *manager,OP_HANDLE *op,unsig TDynManagerBulkOp *current_op; unsigned char i,j,device_index; + dyn_manager_lock(manager); for(i=0;i<manager->num_masters;i++) { if(op->op_index[i]!=0xFF) @@ -890,6 +960,7 @@ void dyn_manager_bulk_op_change_address(TDynManager *manager,OP_HANDLE *op,unsig } } } + dyn_manager_unlock(manager); } void dyn_manager_bulk_op_change_length(TDynManager *manager,OP_HANDLE *op,unsigned char num,unsigned char *ids,unsigned short int *length) @@ -897,6 +968,7 @@ void dyn_manager_bulk_op_change_length(TDynManager *manager,OP_HANDLE *op,unsign TDynManagerBulkOp *current_op; unsigned char i,j,device_index; + dyn_manager_lock(manager); for(i=0;i<manager->num_masters;i++) { if(op->op_index[i]!=0xFF) @@ -909,6 +981,7 @@ void dyn_manager_bulk_op_change_length(TDynManager *manager,OP_HANDLE *op,unsign } } } + dyn_manager_unlock(manager); } void dyn_manager_bulk_op_change_data(TDynManager *manager,OP_HANDLE *op,unsigned char num,unsigned char *ids,unsigned char * const data[]) @@ -916,6 +989,7 @@ void dyn_manager_bulk_op_change_data(TDynManager *manager,OP_HANDLE *op,unsigned TDynManagerBulkOp *current_op; unsigned char i,j,device_index; + dyn_manager_lock(manager); for(i=0;i<manager->num_masters;i++) { if(op->op_index[i]!=0xFF) @@ -928,6 +1002,7 @@ void dyn_manager_bulk_op_change_data(TDynManager *manager,OP_HANDLE *op,unsigned } } } + dyn_manager_unlock(manager); } void dyn_manager_loop(TDynManager *manager) @@ -945,16 +1020,6 @@ void dyn_manager_loop(TDynManager *manager) 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}; - // do scan - if(manager->do_scan) - { - manager->do_scan=0x00; -// dyn_manager_stop(manager); - dyn_manager_scan(manager); -// dyn_manager_start(manager); - return; - } - // initialize the remaining operations for(i=0;i<manager->num_masters;i++) { @@ -980,6 +1045,7 @@ void dyn_manager_loop(TDynManager *manager) } // send all the commands + dyn_manager_lock(manager); do{ for(i=0;i<manager->num_masters;i++) { @@ -1110,6 +1176,7 @@ 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++)