sync read bug fixed

AshisGhosh · Jan 24, 2019 · 0fd3702 · 0fd3702
1 parent ab64859
commit 0fd3702
Showing 1 changed file with 1 addition and 1 deletion.
diff --git a/dynamixel_workbench_controllers/src/dynamixel_workbench_controllers.cpp b/dynamixel_workbench_controllers/src/dynamixel_workbench_controllers.cpp
@@ -220,7 +220,7 @@ bool DynamixelController::initSDKHandlers(void)
   if (dxl_wb_->getProtocolVersion() == 2.0f)
   {  
     uint16_t start_address = std::min(control_items_["Present_Position"]->address, control_items_["Present_Current"]->address);
-    uint16_t read_length = abs(control_items_["Present_Position"]->address - control_items_["Present_Current"]->address) + control_items_["Present_Current"]->data_length;
+    uint16_t read_length = control_items_["Present_Position"]->data_length + control_items_["Present_Velocity"]->data_length + control_items_["Present_Current"]->data_length;
 
     result = dxl_wb_->addSyncReadHandler(start_address,
                                           read_length,