@@ -635,6 +635,8 @@ std::string Dynamixel::DxlErrorToString(DxlError error_num)
635
635
return " DLX_HARDWARE_ERROR" ;
636
636
case DXL_REBOOT_FAIL:
637
637
return " DXL_REBOOT_FAIL" ;
638
+ default :
639
+ return " UNKNOWN_ERROR" ;
638
640
}
639
641
}
640
642
@@ -965,15 +967,15 @@ DxlError Dynamixel::ProcessReadCommunication(
965
967
dxl_comm_result = group_sync_read->txPacket ();
966
968
if (dxl_comm_result != COMM_SUCCESS) {
967
969
fprintf (
968
- stderr, " SyncRead Tx Fail [Dxl Size : %d ] [Error code : %d]\n " ,
970
+ stderr, " SyncRead Tx Fail [Dxl Size : %ld ] [Error code : %d]\n " ,
969
971
read_data_list_.size (), dxl_comm_result);
970
972
return DxlError::SYNC_READ_FAIL;
971
973
}
972
974
} else {
973
975
dxl_comm_result = group_bulk_read->txPacket ();
974
976
if (dxl_comm_result != COMM_SUCCESS) {
975
977
fprintf (
976
- stderr, " BulkRead Tx Fail [Dxl Size : %d ] [Error code : %d]\n " ,
978
+ stderr, " BulkRead Tx Fail [Dxl Size : %ld ] [Error code : %d]\n " ,
977
979
read_data_list_.size (), dxl_comm_result);
978
980
return DxlError::BULK_READ_FAIL;
979
981
}
@@ -989,15 +991,15 @@ DxlError Dynamixel::ProcessReadCommunication(
989
991
dxl_comm_result = group_sync_read->rxPacket ();
990
992
if (dxl_comm_result != COMM_SUCCESS) {
991
993
fprintf (
992
- stderr, " SyncRead Rx Fail [Dxl Size : %d ] [Error code : %d]\n " ,
994
+ stderr, " SyncRead Rx Fail [Dxl Size : %ld ] [Error code : %d]\n " ,
993
995
read_data_list_.size (), dxl_comm_result);
994
996
return DxlError::SYNC_READ_FAIL;
995
997
}
996
998
} else {
997
999
dxl_comm_result = group_bulk_read->rxPacket ();
998
1000
if (dxl_comm_result != COMM_SUCCESS) {
999
1001
fprintf (
1000
- stderr, " BulkRead Rx Fail [Dxl Size : %d ] [Error code : %d]\n " ,
1002
+ stderr, " BulkRead Rx Fail [Dxl Size : %ld ] [Error code : %d]\n " ,
1001
1003
read_data_list_.size (), dxl_comm_result);
1002
1004
return DxlError::BULK_READ_FAIL;
1003
1005
}
@@ -1027,9 +1029,8 @@ DxlError Dynamixel::ProcessReadData(
1027
1029
id,
1028
1030
static_cast <int32_t >(dxl_getdata));
1029
1031
} else if (item_names[item_index] == " Present Velocity" ) {
1030
- *data_ptrs[item_index] = dxl_info_.ConvertValueRPMToVelocityRPS (
1031
- id,
1032
- static_cast <int32_t >(dxl_getdata));
1032
+ *data_ptrs[item_index] =
1033
+ dxl_info_.ConvertValueRPMToVelocityRPS (static_cast <int32_t >(dxl_getdata));
1033
1034
} else if (item_names[item_index] == " Present Current" ) {
1034
1035
*data_ptrs[item_index] = dxl_info_.ConvertCurrentToEffort (
1035
1036
id,
@@ -1173,7 +1174,7 @@ DxlError Dynamixel::SetDxlValueToSyncWrite()
1173
1174
param_write_value[added_byte + 0 ] = DXL_LOBYTE (goal_current);
1174
1175
param_write_value[added_byte + 1 ] = DXL_HIBYTE (goal_current);
1175
1176
} else if (indirect_info_write_[ID].item_name .at (item_index) == " Goal Velocity" ) {
1176
- int16_t goal_velocity = dxl_info_.ConvertVelocityRPSToValueRPM (ID, data);
1177
+ int16_t goal_velocity = dxl_info_.ConvertVelocityRPSToValueRPM (data);
1177
1178
param_write_value[added_byte + 0 ] = DXL_LOBYTE (DXL_LOWORD (goal_velocity));
1178
1179
param_write_value[added_byte + 1 ] = DXL_HIBYTE (DXL_LOWORD (goal_velocity));
1179
1180
param_write_value[added_byte + 2 ] = DXL_LOBYTE (DXL_HIWORD (goal_velocity));
@@ -1288,7 +1289,7 @@ DxlError Dynamixel::SetDxlValueToBulkWrite()
1288
1289
param_write_value[added_byte + 0 ] = DXL_LOBYTE (goal_current);
1289
1290
param_write_value[added_byte + 1 ] = DXL_HIBYTE (goal_current);
1290
1291
} else if (indirect_info_write_[ID].item_name .at (item_index) == " Goal Velocity" ) {
1291
- int32_t goal_velocity = dxl_info_.ConvertVelocityRPSToValueRPM (ID, data);
1292
+ int32_t goal_velocity = dxl_info_.ConvertVelocityRPSToValueRPM (data);
1292
1293
param_write_value[added_byte + 0 ] = DXL_LOBYTE (DXL_LOWORD (goal_velocity));
1293
1294
param_write_value[added_byte + 1 ] = DXL_HIBYTE (DXL_LOWORD (goal_velocity));
1294
1295
param_write_value[added_byte + 2 ] = DXL_LOBYTE (DXL_HIWORD (goal_velocity));
0 commit comments