Added the base address of the EEPROM segments to the slave device module.

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
    *

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