diff --git a/dynamixel_manager/Makefile b/dynamixel_manager/Makefile index 194e7fca6f45c5a0178292102386447f8f05e892..1eb0622aba6cb3ccd78e5a4df4867c0c583003d3 100755 --- a/dynamixel_manager/Makefile +++ b/dynamixel_manager/Makefile @@ -15,8 +15,9 @@ COMM_PATH = ../comm UTILS_PATH = ../utils DYN_BASE_PATH = ../dynamixel_base MEMORY_PATH = ../memory +SCHEDULER_PATH = ../scheduler -INCLUDE_DIRS = -I./include/ -I./include/modules -I$(COMM_PATH)/include -I$(UTILS_PATH)/include -I$(DYN_BASE_PATH)/include -I$(MEMORY_PATH)/include +INCLUDE_DIRS = -I./include/ -I./include/modules -I$(COMM_PATH)/include -I$(UTILS_PATH)/include -I$(DYN_BASE_PATH)/include -I$(MEMORY_PATH)/include -I$(SCHEDULER_PATH)/include MACROS = -DMAX_DYN_MASTER_TX_BUFFER_LEN=256 -DMAX_DYN_MASTER_RX_BUFFER_LEN=256 -DDYN_MANAGER_MAX_NUM_MASTERS=4 -DDYN_MANAGER_MAX_NUM_MODULES=8 -DDYN_MANAGER_MAX_NUM_DEVICES=32 -DDYN_MANAGER_MAX_NUM_SINGLE_OP=16 -DDYN_MANAGER_MAX_NUM_SYNC_OP=4 -DDYN_MANAGER_MAX_NUM_BULK_OP=4 -DMODULE_MAX_NUM_MODELS=32 -DMM_MAX_NUM_MOTION_MODULES=8 @@ -37,6 +38,8 @@ SRC_DIR=./src/ SRC=$(wildcard $(SRC_DIR)*.c) SRC_DIR_MODULES=./src/modules/ SRC+=$(wildcard $(SRC_DIR_MODULES)*.c) +SRC_DIR_SCHEDULER=$(SCHEDULER_PATH)/src +SRC+=$(wildcard $(SRC_DIR_SCHEDULER)*.c) DYNAMIXEL_M4_FPU_OBJ_DIR=build/m4_fpu/ DYNAMIXEL_M4_FPU_OBJS_TMP = $(notdir $(SRC:.c=.o)) diff --git a/dynamixel_manager/include/dyn_manager.h b/dynamixel_manager/include/dyn_manager.h index 24f77938df9fd8e7ba0e8f56c07e804094a1b06f..58cbc68136ee42181371e28dc62c69768c049a7a 100644 --- a/dynamixel_manager/include/dyn_manager.h +++ b/dynamixel_manager/include/dyn_manager.h @@ -8,6 +8,7 @@ extern "C" { #include "dyn_manager_registers.h" #include "dynamixel_master.h" #include "memory.h" +#include "scheduler.h" #ifndef DYN_MANAGER_MAX_NUM_MASTERS #error "Please, specify the maximum number of masters with the DYN_MANAGER_MAX_NUM_MASTERS macro" @@ -116,19 +117,19 @@ typedef struct{ unsigned char num_devices; OP_HANDLE op_handles[DYN_MANAGER_MAX_NUM_OP]; unsigned char num_ops; - unsigned short int period_us; - void (*init_timer)(void); - void (*set_period)(unsigned short int period_us); + unsigned char period_ms; + TScheduler *scheduler; + sched_channel_t sch_channel; TMemory *memory; TMemModule mem_module; }TDynManager; // public functions -unsigned char dyn_manager_init(TDynManager *manager,TMemory *memory,unsigned short int eeprom_base_address,unsigned short int ram_base_address); -void dyn_manager_set_period(TDynManager *manager,unsigned short int period_us); -static inline unsigned short int dyn_manager_get_period(TDynManager *manager) +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); +void dyn_manager_set_period(TDynManager *manager,unsigned char period_ms); +static inline unsigned char dyn_manager_get_period(TDynManager *manager) { - return manager->period_us; + return manager->period_ms; } void dyn_manager_scan(TDynManager *manager); void dyn_manager_add_master(TDynManager *manager,TDynamixelMaster *master); diff --git a/dynamixel_manager/include/dyn_manager_registers.h b/dynamixel_manager/include/dyn_manager_registers.h index c7d360be6c57731f044ba572f5d6af9b17b51455..ee69d217a15315343ef19b75e13d22461baf56a6 100644 --- a/dynamixel_manager/include/dyn_manager_registers.h +++ b/dynamixel_manager/include/dyn_manager_registers.h @@ -3,7 +3,7 @@ #define RAM_DYN_MANAGER_LENGTH 2 -#define EEPROM_DYN_MANAGER_LENGTH 2 +#define EEPROM_DYN_MANAGER_LENGTH 1 #define DYN_MANAGER_PERIOD 0 @@ -11,8 +11,7 @@ #define DYN_MANAGER_NUM_MASTERS 1 #define dyn_manager_eeprom_data(name,section_name,base_address,DEFAULT_PERIOD) \ -unsigned short int name##_eeprom_data[] __attribute__ ((section (section_name)))={DEFAULT_PERIOD&0x00FF,base_address+DYN_MANAGER_PERIOD, \ - (DEFAULT_PERIOD>>8)&0x00FF,base_address+DYN_MANAGER_PERIOD+1}; +unsigned short int name##_eeprom_data[] __attribute__ ((section (section_name)))={DEFAULT_PERIOD&0x00FF,base_address+DYN_MANAGER_PERIOD}; #endif diff --git a/dynamixel_manager/include/dyn_module.h b/dynamixel_manager/include/dyn_module.h index 8620ebe0c9590c6e1e6a64c08a7f94d4e85ae2c9..0b3750012ddb5d977cbc9640a4cbad751ec2c588 100644 --- a/dynamixel_manager/include/dyn_module.h +++ b/dynamixel_manager/include/dyn_module.h @@ -21,7 +21,7 @@ typedef struct TDynModule{ unsigned char period_count; unsigned char enabled; void (*add_device)(void *module_data,unsigned char id,unsigned short int model); - void (*set_period)(void *module_data,unsigned short int period_us); + void (*set_period)(void *module_data,unsigned short int period_ms); void (*setup)(void *module_data); void (*pre_process)(void *module_data); void (*post_process)(void *module_data); diff --git a/dynamixel_manager/include/modules/motion_module.h b/dynamixel_manager/include/modules/motion_module.h index 9498d5411c91ed6d61991c24e623ecb7966302ae..af07a7d31d89bd8abd16f83470cb0ffad9d42905 100644 --- a/dynamixel_manager/include/modules/motion_module.h +++ b/dynamixel_manager/include/modules/motion_module.h @@ -10,7 +10,7 @@ extern "C" { typedef struct TMotionModule{ TModules id; TMotionManager *manager; - void (*set_period)(void *module,unsigned short int period_us); + void (*set_period)(void *module,unsigned short int period_ms); void (*set_module)(void *module,unsigned char servo_id); void (*pre_process)(void *module); void (*post_process)(void *module); diff --git a/dynamixel_manager/src/dyn_manager.c b/dynamixel_manager/src/dyn_manager.c index 696085e5205e58511b193d13546a46c7914b3c84..18046e5b85501aae1be41a863c3fe3fcecdbcfbb 100644 --- a/dynamixel_manager/src/dyn_manager.c +++ b/dynamixel_manager/src/dyn_manager.c @@ -20,18 +20,16 @@ typedef enum {start_single_ops,wait_single_ops,start_sync_ops,wait_sync_ops,star // private functions void dyn_manager_write_cmd(TDynManager *module,unsigned short int address,unsigned short int length,unsigned char *data) { - unsigned short int uint16_value; + unsigned char uint8_value; unsigned char *data_ptr; - if(ram_in_window(DYN_MANAGER_PERIOD,2,address,length)) + if(ram_in_window(DYN_MANAGER_PERIOD,1,address,length)) { - uint16_value=dyn_manager_get_period(module); - data_ptr=(unsigned char *)&uint16_value; + uint8_value=dyn_manager_get_period(module); + data_ptr=(unsigned char *)&uint8_value; if(ram_in_range(DYN_MANAGER_PERIOD,address,length)) data_ptr[0]=data[DYN_MANAGER_PERIOD-address]; - if(ram_in_range(DYN_MANAGER_PERIOD+1,address,length)) - data_ptr[1]=data[DYN_MANAGER_PERIOD+1-address]; - dyn_manager_set_period(module,uint16_value); + dyn_manager_set_period(module,uint8_value); } } @@ -211,7 +209,7 @@ void dyn_manager_delete_op_single_master(TDynManager *manager,unsigned char mast } // public functions -unsigned char dyn_manager_init(TDynManager *manager,TMemory *memory,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 sch_channel,unsigned short int eeprom_base_address,unsigned short int ram_base_address) { unsigned char i,j,k; @@ -281,9 +279,10 @@ unsigned char dyn_manager_init(TDynManager *manager,TMemory *memory,unsigned sho manager->op_handles[i].handle_index=i; } manager->num_ops=0x00; - /* initialize timer */ - if(manager->init_timer!=0x00000000) - manager->init_timer(); + /* initialize scheduler */ + scheduler_set_channel(scheduler,sch_channel,(void(*)(void *))dyn_manager_loop,100,manager); + manager->scheduler=scheduler; + manager->sch_channel=sch_channel; /* initialize memory module */ mem_module_init(&manager->mem_module); @@ -303,20 +302,16 @@ unsigned char dyn_manager_init(TDynManager *manager,TMemory *memory,unsigned sho return 0x01; } -void dyn_manager_set_period(TDynManager *manager,unsigned short int period_us) +void dyn_manager_set_period(TDynManager *manager,unsigned char period_ms) { unsigned char i; - if(manager->set_period!=0x00000000) - { - manager->set_period(period_us); - 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_us); - manager->period_us=period_us; - manager->memory->data[DYN_MANAGER_PERIOD]=period_us%256; - manager->memory->data[DYN_MANAGER_PERIOD+1]=period_us/256; - } + scheduler_change_period(manager->scheduler,manager->sch_channel,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); + manager->period_ms=period_ms; + manager->memory->data[DYN_MANAGER_PERIOD]=period_ms; } void dyn_manager_scan(TDynManager *manager) @@ -390,6 +385,7 @@ void dyn_manager_add_module(TDynManager *manager,TDynModule *module) manager->num_modules++; module->manager=manager; manager->memory->data[DYN_MANAGER_NUM_MODULES]++; + manager->modules[i]->set_period(manager->modules[i]->data,manager->period_ms*manager->modules[i]->period_count); } } diff --git a/dynamixel_manager/src/dyn_module.c b/dynamixel_manager/src/dyn_module.c index e61cd7c22c200c882319581bf13bc111d5a79f92..1cf92bd7d4fe269ca9774c48e4fb004e07b61630 100644 --- a/dynamixel_manager/src/dyn_module.c +++ b/dynamixel_manager/src/dyn_module.c @@ -13,6 +13,7 @@ unsigned char dyn_module_init(TDynModule *module) module->period_count=0x00; module->enabled=0x00; module->add_device=0x00000000; + module->set_period=0x00000000; module->setup=0x00000000; module->pre_process=0x00000000; module->post_process=0x00000000; @@ -54,7 +55,7 @@ void dyn_module_add_device(TDynModule *module,unsigned char id, unsigned short i } } -unsigned char dyn_moudle_device_is_assigned(TDynModule *module,unsigned char id) +unsigned char dyn_module_device_is_assigned(TDynModule *module,unsigned char id) { if(id<DYN_MANAGER_MAX_NUM_DEVICES) return module->assigned_ids[id]; @@ -65,7 +66,8 @@ unsigned char dyn_moudle_device_is_assigned(TDynModule *module,unsigned char id) void dyn_module_set_period(TDynModule *module,unsigned char period) { if(period==0x00) - module->period_count=0x01; - else - module->period_count=period; + period=0x01; + module->period_count=period; + if(module->set_period!=0x00000000) + module->set_period(module->data,period*module->manager->period_ms); } diff --git a/dynamixel_manager/src/modules/action.c b/dynamixel_manager/src/modules/action.c index 978ff509dc9e608978659f9671ad47e39506b28b..da86b52a7eef5b178ed98bafd7d6adfdcbeda15c 100755 --- a/dynamixel_manager/src/modules/action.c +++ b/dynamixel_manager/src/modules/action.c @@ -224,10 +224,10 @@ void action_finish(void) action_running=0x00; } -void action_set_period(void *module,unsigned short int period_us) +void action_set_period(void *module,unsigned short int period_ms) { // load the current period - action_period=(period_us<<16)/1000000; + action_period=(period_ms<<16)/1000; } void action_set_module(void *module,unsigned char servo_id) diff --git a/dynamixel_manager/src/modules/motion_manager.c b/dynamixel_manager/src/modules/motion_manager.c index 1a4363bd5dddff54b116de15062642c894bb49e9..ae78e45e9193180b609288c959c116efeda76365 100644 --- a/dynamixel_manager/src/modules/motion_manager.c +++ b/dynamixel_manager/src/modules/motion_manager.c @@ -229,13 +229,13 @@ void mmanager_setup(TMotionManager *mmanager) mmanager->feedback_op=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 int period_us) +void mmanager_set_period(TMotionManager *mmanager,unsigned short period_ms) { unsigned char i; for(i=0;i<MM_MAX_NUM_MOTION_MODULES;i++) - if(mmanager->modules[i]->set_period!=0x00000000) - mmanager->modules[i]->set_period(mmanager->modules[i]->data,period_us*mmanager->dyn_module.period_count); + if(mmanager->modules[i]!=0x00000000 && mmanager->modules[i]->set_period!=0x00000000) + mmanager->modules[i]->set_period(mmanager->modules[i]->data,period_ms); } void mmanager_pre_process(TMotionManager *mmanager)