diff --git a/dynamixel_manager/include/dyn_manager_registers.h b/dynamixel_manager/include/dyn_manager_registers.h index ee69d217a15315343ef19b75e13d22461baf56a6..70940a8207dd39b2d6986d733f15f9d941666747 100644 --- a/dynamixel_manager/include/dyn_manager_registers.h +++ b/dynamixel_manager/include/dyn_manager_registers.h @@ -5,10 +5,10 @@ #define EEPROM_DYN_MANAGER_LENGTH 1 -#define DYN_MANAGER_PERIOD 0 +#define DYN_MANAGER_PERIOD_OFFSET 0 -#define DYN_MANAGER_NUM_MODULES 0 -#define DYN_MANAGER_NUM_MASTERS 1 +#define DYN_MANAGER_NUM_MODULES_OFFSET 0 +#define DYN_MANAGER_NUM_MASTERS_OFFSET 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}; diff --git a/dynamixel_manager/include/dyn_module.h b/dynamixel_manager/include/dyn_module.h index 0b3750012ddb5d977cbc9640a4cbad751ec2c588..9291e3470a0452da3802b640186d6751b44fe4fa 100644 --- a/dynamixel_manager/include/dyn_module.h +++ b/dynamixel_manager/include/dyn_module.h @@ -26,13 +26,14 @@ typedef struct TDynModule{ void (*pre_process)(void *module_data); void (*post_process)(void *module_data); void *data; + TMemory *memory; TMemModule mem_module; unsigned short int ram_base_address; unsigned short int eeprom_base_address; }TDynModule; //public functions -unsigned char dyn_module_init(TDynModule *module); +unsigned char dyn_module_init(TDynModule *module,TMemory *memory,unsigned short int ram_base_address, unsigned short int eeprom_base_address); static inline void dyn_module_enable(TDynModule *module) { module->enabled=0x01; diff --git a/dynamixel_manager/include/dyn_module_registers.h b/dynamixel_manager/include/dyn_module_registers.h index fe27dbb309ebefbe6eb880dd716b8ce44518836e..0269bbecbf8665f8d115641befcc5ddc333918c2 100644 --- a/dynamixel_manager/include/dyn_module_registers.h +++ b/dynamixel_manager/include/dyn_module_registers.h @@ -1,15 +1,14 @@ #ifndef _DYN_MODULE_REGISTERS_H #define _DYN_MODULE_REGISTERS_H -#define RAM_DYN_MODULE_LENGTH 3 +#define RAM_DYN_MODULE_LENGTH 2 #define EEPROM_DYN_MODULE_LENGTH 1 -#define DYN_MODULE_PERIOD 0 +#define DYN_MODULE_PERIOD_OFFSET 0 -#define DYN_MODULE_CNTRL 0 -#define DYN_MODULE_NUM_MODELS 1 -#define DYN_MODULE_NUM_DEVICES 2 +#define DYN_MODULE_NUM_MODELS_OFFSET 0 +#define DYN_MODULE_NUM_DEVICES_OFFSET 1 #define dyn_module_eeprom_data(name,section_name,base_address,DEFAULT_PERIOD) \ unsigned short int name##_eeprom_data[] __attribute__ ((section (section_name)))={DEFAULT_PERIOD,base_address+DYN_MODULE_PERIOD}; diff --git a/dynamixel_manager/include/modules/motion_manager.h b/dynamixel_manager/include/modules/motion_manager.h index 268c0e8b6ead57dd84eb66bf1f2f283b83430b32..6f09e0c42ebe6e1f1fd0a6528c5f5de40ab523a3 100644 --- a/dynamixel_manager/include/modules/motion_manager.h +++ b/dynamixel_manager/include/modules/motion_manager.h @@ -43,6 +43,10 @@ typedef struct { OP_HANDLE *feedback_op[2]; unsigned char balance_enabled; void (*balance)(short int offsets[DYN_MANAGER_MAX_NUM_DEVICES]); + TMemory *memory; + TMemModule mem_module; + unsigned short int ram_base_address; + unsigned short int eeprom_base_address; }TMotionManager; unsigned char mmanager_init(TMotionManager *mmanager,TMemory *memory,unsigned short int ram_base_address,unsigned short int eeprom_base_address); diff --git a/dynamixel_manager/include/modules/motion_manager_registers.h b/dynamixel_manager/include/modules/motion_manager_registers.h index dcb5838b0f3a2f0dcc3e0e75488acbba1ea2398d..3fa14283eabbd27209c92d2bc0b5e67c5c0d6b1e 100644 --- a/dynamixel_manager/include/modules/motion_manager_registers.h +++ b/dynamixel_manager/include/modules/motion_manager_registers.h @@ -3,13 +3,13 @@ #include "dyn_module_registers.h" -#define RAM_MM_LENGTH (RAM_DYN_MODULE_LENGTH+DYN_MANAGER_MAX_NUM_DEVICES/2) +#define RAM_MM_LENGTH (DYN_MANAGER_MAX_NUM_DEVICES/2) -#define MM_ENABLE_MODULE 0 +#define MM_ENABLE_MODULE_OFFSET 0 #define EEPROM_MM_LENGTH (DYN_MANAGER_MAX_NUM_DEVICES) -#define MM_SERVO_OFFSET 0 +#define MM_SERVO_OFFSET_OFFSET 0 #define dyn_mm_eeprom_data(name,section_name,base_address,DEFAULT_PERIOD,initial_offset) \ dyn_module_eeprom_data(name,section_name,base_address,DEFAULT_PERIOD) \ diff --git a/dynamixel_manager/src/dyn_manager.c b/dynamixel_manager/src/dyn_manager.c index ba83d0e7b27d1f9364fbd401ebaa2591a1689dbd..9d6ef84a5453a0dd74e9da81c64b3d810fa1bf37 100644 --- a/dynamixel_manager/src/dyn_manager.c +++ b/dynamixel_manager/src/dyn_manager.c @@ -20,17 +20,12 @@ 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 char uint8_value; - unsigned char *data_ptr; + unsigned short int eeprom_offset; - if(ram_in_window(DYN_MANAGER_PERIOD,1,address,length)) - { - 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]; - dyn_manager_set_period(module,uint8_value); - } + eeprom_offset=address-module->eeprom_base_address; + if(ram_in_window(module->eeprom_base_address+DYN_MANAGER_PERIOD_OFFSET,1,address,length)) + dyn_manager_set_period(module,data[DYN_MANAGER_PERIOD_OFFSET-eeprom_offset]); + ram_write_table(module->memory,address,length,data); } void dyn_manager_read_cmd(TDynManager *module,unsigned short int address,unsigned short int length,unsigned char *data) @@ -311,7 +306,6 @@ void dyn_manager_set_period(TDynManager *manager,unsigned char period_ms) 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) @@ -360,7 +354,7 @@ void dyn_manager_add_master(TDynManager *manager,TDynamixelMaster *master) { manager->masters[i]=master; manager->num_masters++; - manager->memory->data[DYN_MANAGER_NUM_MASTERS]++; + manager->memory->data[manager->ram_base_address+DYN_MANAGER_NUM_MASTERS_OFFSET]++; } } @@ -384,7 +378,7 @@ void dyn_manager_add_module(TDynManager *manager,TDynModule *module) manager->modules[i]=module; manager->num_modules++; module->manager=manager; - manager->memory->data[DYN_MANAGER_NUM_MODULES]++; + manager->memory->data[manager->ram_base_address+DYN_MANAGER_NUM_MODULES_OFFSET]++; 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 1cf92bd7d4fe269ca9774c48e4fb004e07b61630..a5c470e85970c1b9fdf16d7aa872392fd9507cbc 100644 --- a/dynamixel_manager/src/dyn_module.c +++ b/dynamixel_manager/src/dyn_module.c @@ -1,6 +1,25 @@ #include "dyn_module.h" +#include "dyn_module_registers.h" +#include "ram.h" -unsigned char dyn_module_init(TDynModule *module) +// private functions +void dyn_module_write_cmd(TDynModule *module,unsigned short int address,unsigned short int length,unsigned char *data) +{ + unsigned short int eeprom_offset; + + eeprom_offset=address-module->eeprom_base_address; + if(ram_in_window(module->eeprom_base_address+DYN_MODULE_PERIOD_OFFSET,1,address,length)) + dyn_module_set_period(module,data[DYN_MODULE_PERIOD_OFFSET-eeprom_offset]); + ram_write_table(module->memory,address,length,data); +} + +void dyn_module_read_cmd(TDynManager *module,unsigned short int address,unsigned short int length,unsigned char *data) +{ + ram_read_table(module->memory,address,length,data); +} + +// public functions +unsigned char dyn_module_init(TDynModule *module,TMemory *memory,unsigned short int ram_base_address,unsigned short int eeprom_base_address) { unsigned char i; @@ -20,6 +39,18 @@ unsigned char dyn_module_init(TDynModule *module) module->data=0x00000000; /* initialize memory module */ mem_module_init(&module->mem_module); + module->mem_module.data=module; + module->mem_module.write_cmd=(void(*)(void *,unsigned short int, unsigned short int,unsigned char *))dyn_module_write_cmd; + module->mem_module.read_cmd=(void(*)(void *,unsigned short int, unsigned short int,unsigned char *))dyn_module_read_cmd; + if(!mem_module_add_ram_segment(&module->mem_module,ram_base_address,RAM_DYN_MODULE_LENGTH)) + return 0x00; + if(!mem_module_add_eeprom_segment(&module->mem_module,eeprom_base_address,EEPROM_DYN_MODULE_LENGTH)) + return 0x00; + if(!mem_add_module(memory,&module->mem_module)) + return 0x00; + module->eeprom_base_address=eeprom_base_address; + module->ram_base_address=ram_base_address; + module->memory=memory; return 0x01; } diff --git a/dynamixel_manager/src/modules/motion_manager.c b/dynamixel_manager/src/modules/motion_manager.c index 256ba553c73cc9b9ccd5a5cbf967cc077997b35c..07fa4b0156b49ec5a31073e806cb4a366e0e86ba 100644 --- a/dynamixel_manager/src/modules/motion_manager.c +++ b/dynamixel_manager/src/modules/motion_manager.c @@ -213,7 +213,7 @@ unsigned char mmanager_init(TMotionManager *mmanager,TMemory *memory,unsigned sh unsigned char i; /* initialize the base module */ - dyn_module_init(&mmanager->dyn_module); + dyn_module_init(&mmanager->dyn_module,memory,ram_base_address,eeprom_base_address); /* add all known servo models */ for(i=0;i<NUM_SERVO_MODELS;i++) dyn_module_add_model(&mmanager->dyn_module,servo_data[i].model); @@ -252,17 +252,19 @@ unsigned char mmanager_init(TMotionManager *mmanager,TMemory *memory,unsigned sh mmanager->balance=0x00000000; /* initialize memory module */ - mmanager->dyn_module.mem_module.data=mmanager; - mmanager->dyn_module.mem_module.write_cmd=mmanager_write_cmd; - mmanager->dyn_module.mem_module.read_cmd=mmanager_read_cmd; - if(!mem_module_add_ram_segment(&mmanager->dyn_module.mem_module,ram_base_address,RAM_MM_LENGTH)) + mem_module_init(&mmanager->mem_module); + mmanager->mem_module.data=mmanager; + mmanager->mem_module.write_cmd=mmanager_write_cmd; + mmanager->mem_module.read_cmd=mmanager_read_cmd; + if(!mem_module_add_ram_segment(&mmanager->mem_module,ram_base_address+RAM_DYN_MODULE_LENGTH,RAM_MM_LENGTH)) return 0x00; - mmanager->dyn_module.ram_base_address=ram_base_address; - if(!mem_module_add_eeprom_segment(&mmanager->dyn_module.mem_module,eeprom_base_address,EEPROM_MM_LENGTH)) + mmanager->ram_base_address=ram_base_address; + if(!mem_module_add_eeprom_segment(&mmanager->mem_module,eeprom_base_address+EEPROM_DYN_MODULE_LENGTH,EEPROM_MM_LENGTH)) return 0x00; - mmanager->dyn_module.eeprom_base_address=eeprom_base_address; - if(!mem_add_module(memory,&mmanager->dyn_module.mem_module)) + mmanager->eeprom_base_address=eeprom_base_address; + if(!mem_add_module(memory,&mmanager->mem_module)) return 0x00; + mmanager->memory=memory; return 0x01; }