From ef6b999802c4d190ff5738ca955d66e0a8c85cab Mon Sep 17 00:00:00 2001
From: Sergi Hernandez Juan <shernand@iri.upc.edu>
Date: Sat, 25 Jan 2020 08:33:31 +0100
Subject: [PATCH] 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.
---
.../include/dyn_manager_registers.h | 6 ++--
dynamixel_manager/include/dyn_module.h | 3 +-
.../include/dyn_module_registers.h | 9 +++--
.../include/modules/motion_manager.h | 4 +++
.../modules/motion_manager_registers.h | 6 ++--
dynamixel_manager/src/dyn_manager.c | 20 ++++-------
dynamixel_manager/src/dyn_module.c | 33 ++++++++++++++++++-
.../src/modules/motion_manager.c | 20 ++++++-----
8 files changed, 66 insertions(+), 35 deletions(-)
diff --git a/dynamixel_manager/include/dyn_manager_registers.h b/dynamixel_manager/include/dyn_manager_registers.h
index ee69d21..70940a8 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 0b37500..9291e34 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 fe27dbb..0269bbe 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 268c0e8..6f09e0c 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 dcb5838..3fa1428 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 ba83d0e..9d6ef84 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 1cf92bd..a5c470e 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 256ba55..07fa4b0 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;
}
--
GitLab