From 6a97ea07ec6c4a0e00023529313ea80e4909559a Mon Sep 17 00:00:00 2001
From: Sergi Hernandez Juan <shernand@iri.upc.edu>
Date: Fri, 26 Jul 2019 12:39:07 +0200
Subject: [PATCH] Added the base address of the EEPROM segments to the slave
device module.
---
.../include/dynamixel_slave_device.h | 10 +++++
dynamixel_base/src/dynamixel_slave_device.c | 37 +++++++++++++------
2 files changed, 35 insertions(+), 12 deletions(-)
diff --git a/dynamixel_base/include/dynamixel_slave_device.h b/dynamixel_base/include/dynamixel_slave_device.h
index ef592d9..c43300b 100644
--- a/dynamixel_base/include/dynamixel_slave_device.h
+++ b/dynamixel_base/include/dynamixel_slave_device.h
@@ -26,6 +26,16 @@ typedef struct
*
*/
unsigned char address;
+ /**
+ * \brief
+ *
+ */
+ unsigned short int eeprom_base_address1;
+ /**
+ * \brief
+ *
+ */
+ unsigned short int eeprom_base_address2;
/**
* \brief
*
diff --git a/dynamixel_base/src/dynamixel_slave_device.c b/dynamixel_base/src/dynamixel_slave_device.c
index dba26c2..de9b3f7 100644
--- a/dynamixel_base/src/dynamixel_slave_device.c
+++ b/dynamixel_base/src/dynamixel_slave_device.c
@@ -4,25 +4,36 @@
/* private functions */
void dyn_slave_device_write_cmd(TDynamixelSlaveDevice *device,unsigned short int address,unsigned short int length,unsigned char *data)
{
- if(ram_in_range(DEVICE_ID,address,length))
+ unsigned short int offset;
+
+ offset=address-device->eeprom_base_address1;
+ if(ram_in_window(device->eeprom_base_address1+DEVICE_MODEL,2,address,length))
+ {
+ if(ram_in_range(device->eeprom_base_address1+DEVICE_MODEL,address,length))
+ ram_data[device->eeprom_base_address1+DEVICE_MODEL]=data[DEVICE_MODEL-offset];
+ if(ram_in_range(device->eeprom_base_address1+DEVICE_MODEL+1,address,length))
+ ram_data[device->eeprom_base_address1+DEVICE_MODEL+1]=data[DEVICE_MODEL+1-offset];
+ }
+ if(ram_in_range(device->eeprom_base_address1+DEVICE_ID,address,length))
{
- dyn_slave_device_set_address(device,data[DEVICE_ID-address]);
- ram_data[DEVICE_ID]=data[DEVICE_ID-address];
+ dyn_slave_device_set_address(device,data[DEVICE_ID-offset]);
+ ram_data[device->eeprom_base_address1+DEVICE_ID]=data[DEVICE_ID-offset];
}
- if(ram_in_range(BAUDRATE,address,length))
+ if(ram_in_range(device->eeprom_base_address1+BAUDRATE,address,length))
{
- dyn_slave_set_baudrate(device->slave,2000000/(data[BAUDRATE-address]+1));
- ram_data[BAUDRATE]=data[BAUDRATE-address];
+ dyn_slave_set_baudrate(device->slave,2000000/(data[BAUDRATE-offset]+1));
+ ram_data[device->eeprom_base_address1+BAUDRATE]=data[BAUDRATE-offset];
}
- if(ram_in_range(RETURN_DELAY,address,length))
+ if(ram_in_range(device->eeprom_base_address1+RETURN_DELAY,address,length))
{
- dyn_slave_device_set_return_delay(device,data[RETURN_DELAY-address]);
- ram_data[RETURN_DELAY]=data[RETURN_DELAY-address];
+ dyn_slave_device_set_return_delay(device,data[RETURN_DELAY-offset]);
+ ram_data[device->eeprom_base_address1+RETURN_DELAY]=data[RETURN_DELAY-offset];
}
- if(ram_in_range(RETURN_LEVEL,address,length))
+ offset=address-device->eeprom_base_address2;
+ if(ram_in_range(device->eeprom_base_address2+RETURN_LEVEL,address,length))
{
- dyn_slave_device_set_return_level(device,data[RETURN_LEVEL-address]);
- ram_data[RETURN_LEVEL]=data[RETURN_LEVEL-address];
+ dyn_slave_device_set_return_level(device,data[RETURN_LEVEL-offset]);
+ ram_data[device->eeprom_base_address2+RETURN_LEVEL]=data[RETURN_LEVEL-offset];
}
}
@@ -403,6 +414,8 @@ unsigned char dyn_slave_device_init(TDynamixelSlaveDevice *device,unsigned char
return 0x00;
if(!mem_add_module(&device->memory,&device->mem_module))
return 0x00;
+ device->eeprom_base_address1=eeprom_base_address1;
+ device->eeprom_base_address2=eeprom_base_address2;
return 0x01;
}
--
GitLab