BRSRI/bellande_humanoid_robot_intelligence_control_system_framework
1
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity

- modified to prevent duplicate indirect address write

Browse Source 
- fixed a bug that occure when handling bulk read item that does not exist.
This commit is contained in:
ROBOTIS-zerom
2017-11-30 17:14:28 +09:00
parent 05f4a5e51c
commit 0076da8a98
2 changed files with 23 additions and 4 deletions
Download Patch File Download Diff File Expand all files Collapse all files

20
robotis_controller/src/robotis_controller/robotis_controller.cpp
View File

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