diff --git a/dynamixel_base/include/dynamixel_slave_device.h b/dynamixel_base/include/dynamixel_slave_device.h index ef592d979b4a95625298e946200e0438d74bc509..c43300b02604378f4a2699ffc3c2d29a0989346c 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 dba26c234c45f3732f4ec0e362e5c94d725f55c3..de9b3f7045f9e2cd9e18de9d50ed437fbe53c755 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; }