From 21e9a81af0d61ea6fd5d7df675934939b9cafd38 Mon Sep 17 00:00:00 2001 From: SCH Date: Wed, 29 Nov 2017 13:51:25 +0900 Subject: [PATCH] - Modified to prevent duplicate indirect address write --- .../robotis_controller/robotis_controller.cpp | 20 +++++++++++++++---- 1 file changed, 16 insertions(+), 4 deletions(-) diff --git a/robotis_controller/src/robotis_controller/robotis_controller.cpp b/robotis_controller/src/robotis_controller/robotis_controller.cpp index be70a4e..691df50 100755 --- a/robotis_controller/src/robotis_controller/robotis_controller.cpp +++ b/robotis_controller/src/robotis_controller/robotis_controller.cpp @@ -485,6 +485,8 @@ void RobotisController::initializeDevice(const std::string init_file_path) { if (dxl->bulk_read_items_.size() != 0) { + uint16_t data16 = 0; + bulkread_start_addr = dxl->bulk_read_items_[0]->address_; bulkread_data_length = 0; @@ -498,7 +500,11 @@ void RobotisController::initializeDevice(const std::string init_file_path) for (int l = 0; l < addr_leng; l++) { // ROS_WARN("[%12s] INDIR_ADDR: %d, ITEM_ADDR: %d", joint_name.c_str(), indirect_addr, dxl->ctrl_table[dxl->bulk_read_items[i]->item_name]->address + _l); - write2Byte(joint_name, indirect_addr, dxl->ctrl_table_[dxl->bulk_read_items_[i]->item_name_]->address_ + l); + read2Byte(joint_name, indirect_addr, &data16); + if (data16 != dxl->ctrl_table_[dxl->bulk_read_items_[i]->item_name_]->address_ + l) + { + write2Byte(joint_name, indirect_addr, dxl->ctrl_table_[dxl->bulk_read_items_[i]->item_name_]->address_ + l); + } indirect_addr += 2; } } @@ -552,6 +558,8 @@ void RobotisController::initializeDevice(const std::string init_file_path) { if (sensor->bulk_read_items_.size() != 0) { + uint16_t data16 = 0; + bulkread_start_addr = sensor->bulk_read_items_[0]->address_; bulkread_data_length = 0; @@ -565,9 +573,13 @@ void RobotisController::initializeDevice(const std::string init_file_path) for (int l = 0; l < addr_leng; l++) { // ROS_WARN("[%12s] INDIR_ADDR: %d, ITEM_ADDR: %d", sensor_name.c_str(), indirect_addr, sensor->ctrl_table[sensor->bulk_read_items[i]->item_name]->address + _l); - write2Byte(sensor_name, - indirect_addr, - sensor->ctrl_table_[sensor->bulk_read_items_[i]->item_name_]->address_ + l); + read2Byte(sensor_name, indirect_addr, &data16); + if (data16 != sensor->ctrl_table_[sensor->bulk_read_items_[i]->item_name_]->address_ + l) + { + write2Byte(sensor_name, + indirect_addr, + sensor->ctrl_table_[sensor->bulk_read_items_[i]->item_name_]->address_ + l); + } indirect_addr += 2; } }