diff --git a/dynamixel_sdk/include/dynamixel_sdk/PacketHandler.h b/dynamixel_sdk/include/dynamixel_sdk/PacketHandler.h index 5a048a5..6edb44e 100644 --- a/dynamixel_sdk/include/dynamixel_sdk/PacketHandler.h +++ b/dynamixel_sdk/include/dynamixel_sdk/PacketHandler.h @@ -9,6 +9,7 @@ #define DYNAMIXEL_SDK_INCLUDE_DYNAMIXEL_SDK_PACKETHANDLER_H_ +#include #include #include "RobotisDef.h" #include "PortHandler.h" @@ -65,6 +66,9 @@ public: virtual float GetProtocolVersion() = 0; + virtual void PrintTxRxResult(int result) = 0; + virtual void PrintRxPacketError(UINT8_T error) = 0; + virtual int TxPacket (PortHandler *port, UINT8_T *txpacket) = 0; virtual int RxPacket (PortHandler *port, UINT8_T *rxpacket) = 0; virtual int TxRxPacket (PortHandler *port, UINT8_T *txpacket, UINT8_T *rxpacket, UINT8_T *error = 0) = 0; diff --git a/dynamixel_sdk/include/dynamixel_sdk/Protocol1PacketHandler.h b/dynamixel_sdk/include/dynamixel_sdk/Protocol1PacketHandler.h index 5f1b2a4..55182f4 100644 --- a/dynamixel_sdk/include/dynamixel_sdk/Protocol1PacketHandler.h +++ b/dynamixel_sdk/include/dynamixel_sdk/Protocol1PacketHandler.h @@ -28,6 +28,9 @@ public: float GetProtocolVersion() { return 1.0; } + void PrintTxRxResult(int result); + void PrintRxPacketError(UINT8_T error); + int TxPacket (PortHandler *port, UINT8_T *txpacket); int RxPacket (PortHandler *port, UINT8_T *rxpacket); int TxRxPacket (PortHandler *port, UINT8_T *txpacket, UINT8_T *rxpacket, UINT8_T *error = 0); diff --git a/dynamixel_sdk/include/dynamixel_sdk/Protocol2PacketHandler.h b/dynamixel_sdk/include/dynamixel_sdk/Protocol2PacketHandler.h index c0ad605..823c226 100644 --- a/dynamixel_sdk/include/dynamixel_sdk/Protocol2PacketHandler.h +++ b/dynamixel_sdk/include/dynamixel_sdk/Protocol2PacketHandler.h @@ -32,6 +32,9 @@ public: float GetProtocolVersion() { return 2.0; } + void PrintTxRxResult(int result); + void PrintRxPacketError(UINT8_T error); + int TxPacket (PortHandler *port, UINT8_T *txpacket); int RxPacket (PortHandler *port, UINT8_T *rxpacket); int TxRxPacket (PortHandler *port, UINT8_T *txpacket, UINT8_T *rxpacket, UINT8_T *error = 0); diff --git a/dynamixel_sdk/src/dynamixel_sdk/GroupBulkRead.cpp b/dynamixel_sdk/src/dynamixel_sdk/GroupBulkRead.cpp index d779039..cab6adc 100644 --- a/dynamixel_sdk/src/dynamixel_sdk/GroupBulkRead.cpp +++ b/dynamixel_sdk/src/dynamixel_sdk/GroupBulkRead.cpp @@ -123,6 +123,7 @@ int GroupBulkRead::RxPacket() { UINT8_T _id = id_list_[_i]; + port_->SetPacketTimeout((UINT16_T)(length_list_[_id] + 11)); _result = ph_->ReadRx(port_, length_list_[_id], data_list_[_id]); if(_result != COMM_SUCCESS) { diff --git a/dynamixel_sdk/src/dynamixel_sdk/Protocol1PacketHandler.cpp b/dynamixel_sdk/src/dynamixel_sdk/Protocol1PacketHandler.cpp index a1a52b6..2b5ccc4 100644 --- a/dynamixel_sdk/src/dynamixel_sdk/Protocol1PacketHandler.cpp +++ b/dynamixel_sdk/src/dynamixel_sdk/Protocol1PacketHandler.cpp @@ -36,6 +36,75 @@ Protocol1PacketHandler *Protocol1PacketHandler::unique_instance_ = new Protocol1 Protocol1PacketHandler::Protocol1PacketHandler() { } +void Protocol1PacketHandler::PrintTxRxResult(int result) +{ + switch(result) + { + case COMM_SUCCESS: + printf("[TxRxResult] Communication success.\n"); + break; + + case COMM_PORT_BUSY: + printf("[TxRxResult] Port is in use!\n"); + break; + + case COMM_TX_FAIL: + printf("[TxRxResult] Failed transmit instruction packet!\n"); + break; + + case COMM_RX_FAIL: + printf("[TxRxResult] Failed get status packet from device!\n"); + break; + + case COMM_TX_ERROR: + printf("[TxRxResult] Incorrect instruction packet!\n"); + break; + + case COMM_RX_WAITING: + printf("[TxRxResult] Now recieving status packet!\n"); + break; + + case COMM_RX_TIMEOUT: + printf("[TxRxResult] There is no status packet!\n"); + break; + + case COMM_RX_CORRUPT: + printf("[TxRxResult] Incorrect status packet!\n"); + break; + + case COMM_NOT_AVAILABLE: + printf("[TxRxResult] Protocol does not support This function!\n"); + break; + + default: + break; + } +} + +void Protocol1PacketHandler::PrintRxPacketError(UINT8_T error) +{ + if(error & ERRBIT_VOLTAGE) + printf("[RxPacketError] Input voltage error!\n"); + + if(error & ERRBIT_ANGLE) + printf("[RxPacketError] Angle limit error!\n"); + + if(error & ERRBIT_OVERHEAT) + printf("[RxPacketError] Overheat error!\n"); + + if(error & ERRBIT_RANGE) + printf("[RxPacketError] Out of range error!\n"); + + if(error & ERRBIT_CHECKSUM) + printf("[RxPacketError] Checksum error!\n"); + + if(error & ERRBIT_OVERLOAD) + printf("[RxPacketError] Overload error!\n"); + + if(error & ERRBIT_INSTRUCTION) + printf("[RxPacketError] Instruction code error!\n"); +} + int Protocol1PacketHandler::TxPacket(PortHandler *port, UINT8_T *txpacket) { UINT8_T _checksum = 0; diff --git a/dynamixel_sdk/src/dynamixel_sdk/Protocol2PacketHandler.cpp b/dynamixel_sdk/src/dynamixel_sdk/Protocol2PacketHandler.cpp index ecdc260..c178f76 100644 --- a/dynamixel_sdk/src/dynamixel_sdk/Protocol2PacketHandler.cpp +++ b/dynamixel_sdk/src/dynamixel_sdk/Protocol2PacketHandler.cpp @@ -42,6 +42,97 @@ Protocol2PacketHandler *Protocol2PacketHandler::unique_instance_ = new Protocol2 Protocol2PacketHandler::Protocol2PacketHandler() { } +void Protocol2PacketHandler::PrintTxRxResult(int result) +{ + switch(result) + { + case COMM_SUCCESS: + printf("[TxRxResult] Communication success.\n"); + break; + + case COMM_PORT_BUSY: + printf("[TxRxResult] Port is in use!\n"); + break; + + case COMM_TX_FAIL: + printf("[TxRxResult] Failed transmit instruction packet!\n"); + break; + + case COMM_RX_FAIL: + printf("[TxRxResult] Failed get status packet from device!\n"); + break; + + case COMM_TX_ERROR: + printf("[TxRxResult] Incorrect instruction packet!\n"); + break; + + case COMM_RX_WAITING: + printf("[TxRxResult] Now recieving status packet!\n"); + break; + + case COMM_RX_TIMEOUT: + printf("[TxRxResult] There is no status packet!\n"); + break; + + case COMM_RX_CORRUPT: + printf("[TxRxResult] Incorrect status packet!\n"); + break; + + case COMM_NOT_AVAILABLE: + printf("[TxRxResult] Protocol does not support This function!\n"); + break; + + default: + break; + } +} + +void Protocol2PacketHandler::PrintRxPacketError(UINT8_T error) +{ + if(error & ERRBIT_ALERT) + printf("[RxPacketError] Hardware error occurred. Check the error at Control Table (Hardware Error Status)!\n"); + + int _error = error & ~ERRBIT_ALERT; + + switch(_error) + { + case 0: + break; + + case ERRNUM_RESULT_FAIL: + printf("[RxPacketError] Failed to process the instruction packet!\n"); + break; + + case ERRNUM_INSTRUCTION: + printf("[RxPacketError] Undefined instruction or incorrect instruction!\n"); + break; + + case ERRNUM_CRC: + printf("[RxPacketError] CRC doesn't match!\n"); + break; + + case ERRNUM_DATA_RANGE: + printf("[RxPacketError] The data value is out of range!\n"); + break; + + case ERRNUM_DATA_LENGTH: + printf("[RxPacketError] The data length does not match as expected!\n"); + break; + + case ERRNUM_DATA_LIMIT: + printf("[RxPacketError] The data value exceeds the limit value!\n"); + break; + + case ERRNUM_ACCESS: + printf("[RxPacketError] Writing or Reading is not available to target address!\n"); + break; + + default: + printf("[RxPacketError] Unknown error code!\n"); + break; + } +} + unsigned short Protocol2PacketHandler::UpdateCRC(UINT16_T crc_accum, UINT8_T *data_blk_ptr, UINT16_T data_blk_size) { UINT16_T i, j;