Solved a bug in the write_cmd() functions to take into account the base addresses.

Each module has its own memory module to handle its own parameters.

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};

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;

dynamixel_manager/include/dyn_module_registers.h

 #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};

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);

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) \

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);
   }
 }

dynamixel_manager/src/dyn_module.c

 #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;
 }

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;
 }