From 6d9aed2691a8785fa1d0e230a09b6377b10a9c3c Mon Sep 17 00:00:00 2001 From: Toreno96 Date: Mon, 27 Mar 2017 19:37:50 +0200 Subject: [PATCH] Resolve ROBOTIS-GIT/DynamixelSDK#67 (C++) --- c++/include/dynamixel_sdk/packet_handler.h | 2 + .../dynamixel_sdk/protocol1_packet_handler.h | 2 + .../dynamixel_sdk/protocol2_packet_handler.h | 2 + .../protocol1_packet_handler.cpp | 79 ++++++++++--------- .../protocol2_packet_handler.cpp | 71 ++++++++--------- 5 files changed, 79 insertions(+), 77 deletions(-) diff --git a/c++/include/dynamixel_sdk/packet_handler.h b/c++/include/dynamixel_sdk/packet_handler.h index c6ea6495..bf7b8b0a 100644 --- a/c++/include/dynamixel_sdk/packet_handler.h +++ b/c++/include/dynamixel_sdk/packet_handler.h @@ -90,7 +90,9 @@ class WINDECLSPEC PacketHandler virtual float getProtocolVersion() = 0; + virtual const char *getTxRxResult(int result) = 0; virtual void printTxRxResult(int result) = 0; + virtual const char *getRxPacketError(uint8_t error) = 0; virtual void printRxPacketError(uint8_t error) = 0; virtual int txPacket (PortHandler *port, uint8_t *txpacket) = 0; diff --git a/c++/include/dynamixel_sdk/protocol1_packet_handler.h b/c++/include/dynamixel_sdk/protocol1_packet_handler.h index 1023207d..44617d58 100644 --- a/c++/include/dynamixel_sdk/protocol1_packet_handler.h +++ b/c++/include/dynamixel_sdk/protocol1_packet_handler.h @@ -53,7 +53,9 @@ class WINDECLSPEC Protocol1PacketHandler : public PacketHandler float getProtocolVersion() { return 1.0; } + const char *getTxRxResult(int result); void printTxRxResult(int result); + const char *getRxPacketError(uint8_t error); void printRxPacketError(uint8_t error); int txPacket (PortHandler *port, uint8_t *txpacket); diff --git a/c++/include/dynamixel_sdk/protocol2_packet_handler.h b/c++/include/dynamixel_sdk/protocol2_packet_handler.h index 0924b0e5..5e4efb78 100644 --- a/c++/include/dynamixel_sdk/protocol2_packet_handler.h +++ b/c++/include/dynamixel_sdk/protocol2_packet_handler.h @@ -57,7 +57,9 @@ class WINDECLSPEC Protocol2PacketHandler : public PacketHandler float getProtocolVersion() { return 2.0; } + const char *getTxRxResult(int result); void printTxRxResult(int result); + const char *getRxPacketError(uint8_t error); void printRxPacketError(uint8_t error); int txPacket (PortHandler *port, uint8_t *txpacket); diff --git a/c++/src/dynamixel_sdk/protocol1_packet_handler.cpp b/c++/src/dynamixel_sdk/protocol1_packet_handler.cpp index 68d22db0..c3df3fc5 100644 --- a/c++/src/dynamixel_sdk/protocol1_packet_handler.cpp +++ b/c++/src/dynamixel_sdk/protocol1_packet_handler.cpp @@ -65,73 +65,76 @@ Protocol1PacketHandler *Protocol1PacketHandler::unique_instance_ = new Protocol1 Protocol1PacketHandler::Protocol1PacketHandler() { } -void Protocol1PacketHandler::printTxRxResult(int result) +const char *Protocol1PacketHandler::getTxRxResult(int result) { switch(result) { - case COMM_SUCCESS: - printf("[TxRxResult] Communication success.\n"); - break; + case COMM_SUCCESS: + return "[TxRxResult] Communication success."; - case COMM_PORT_BUSY: - printf("[TxRxResult] Port is in use!\n"); - break; + case COMM_PORT_BUSY: + return "[TxRxResult] Port is in use!"; - case COMM_TX_FAIL: - printf("[TxRxResult] Failed transmit instruction packet!\n"); - break; + case COMM_TX_FAIL: + return "[TxRxResult] Failed transmit instruction packet!"; - case COMM_RX_FAIL: - printf("[TxRxResult] Failed get status packet from device!\n"); - break; + case COMM_RX_FAIL: + return "[TxRxResult] Failed get status packet from device!"; - case COMM_TX_ERROR: - printf("[TxRxResult] Incorrect instruction packet!\n"); - break; + case COMM_TX_ERROR: + return "[TxRxResult] Incorrect instruction packet!"; - case COMM_RX_WAITING: - printf("[TxRxResult] Now recieving status packet!\n"); - break; + case COMM_RX_WAITING: + return "[TxRxResult] Now recieving status packet!"; - case COMM_RX_TIMEOUT: - printf("[TxRxResult] There is no status packet!\n"); - break; + case COMM_RX_TIMEOUT: + return "[TxRxResult] There is no status packet!"; - case COMM_RX_CORRUPT: - printf("[TxRxResult] Incorrect status packet!\n"); - break; + case COMM_RX_CORRUPT: + return "[TxRxResult] Incorrect status packet!"; - case COMM_NOT_AVAILABLE: - printf("[TxRxResult] Protocol does not support This function!\n"); - break; + case COMM_NOT_AVAILABLE: + return "[TxRxResult] Protocol does not support This function!"; - default: - break; + default: + return ""; } } -void Protocol1PacketHandler::printRxPacketError(uint8_t error) +void Protocol1PacketHandler::printTxRxResult(int result) +{ + printf("%s\n", getTxRxResult(result)); +} + +const char *Protocol1PacketHandler::getRxPacketError(uint8_t error) { if (error & ERRBIT_VOLTAGE) - printf("[RxPacketError] Input voltage error!\n"); + return "[RxPacketError] Input voltage error!"; if (error & ERRBIT_ANGLE) - printf("[RxPacketError] Angle limit error!\n"); + return "[RxPacketError] Angle limit error!"; if (error & ERRBIT_OVERHEAT) - printf("[RxPacketError] Overheat error!\n"); + return "[RxPacketError] Overheat error!"; if (error & ERRBIT_RANGE) - printf("[RxPacketError] Out of range error!\n"); + return "[RxPacketError] Out of range error!"; if (error & ERRBIT_CHECKSUM) - printf("[RxPacketError] Checksum error!\n"); + return "[RxPacketError] Checksum error!"; if (error & ERRBIT_OVERLOAD) - printf("[RxPacketError] Overload error!\n"); + return "[RxPacketError] Overload error!"; if (error & ERRBIT_INSTRUCTION) - printf("[RxPacketError] Instruction code error!\n"); + return "[RxPacketError] Instruction code error!"; + + return ""; +} + +void Protocol1PacketHandler::printRxPacketError(uint8_t error) +{ + printf("%s\n", getRxPacketError(error)); } int Protocol1PacketHandler::txPacket(PortHandler *port, uint8_t *txpacket) diff --git a/c++/src/dynamixel_sdk/protocol2_packet_handler.cpp b/c++/src/dynamixel_sdk/protocol2_packet_handler.cpp index 12ab01b3..17eef51d 100644 --- a/c++/src/dynamixel_sdk/protocol2_packet_handler.cpp +++ b/c++/src/dynamixel_sdk/protocol2_packet_handler.cpp @@ -71,97 +71,90 @@ Protocol2PacketHandler *Protocol2PacketHandler::unique_instance_ = new Protocol2 Protocol2PacketHandler::Protocol2PacketHandler() { } -void Protocol2PacketHandler::printTxRxResult(int result) +const char *Protocol2PacketHandler::getTxRxResult(int result) { switch(result) { case COMM_SUCCESS: - printf("[TxRxResult] Communication success.\n"); - break; + return "[TxRxResult] Communication success."; case COMM_PORT_BUSY: - printf("[TxRxResult] Port is in use!\n"); - break; + return "[TxRxResult] Port is in use!"; case COMM_TX_FAIL: - printf("[TxRxResult] Failed transmit instruction packet!\n"); - break; + return "[TxRxResult] Failed transmit instruction packet!"; case COMM_RX_FAIL: - printf("[TxRxResult] Failed get status packet from device!\n"); - break; + return "[TxRxResult] Failed get status packet from device!"; case COMM_TX_ERROR: - printf("[TxRxResult] Incorrect instruction packet!\n"); - break; + return "[TxRxResult] Incorrect instruction packet!"; case COMM_RX_WAITING: - printf("[TxRxResult] Now recieving status packet!\n"); - break; + return "[TxRxResult] Now recieving status packet!"; case COMM_RX_TIMEOUT: - printf("[TxRxResult] There is no status packet!\n"); - break; + return "[TxRxResult] There is no status packet!"; case COMM_RX_CORRUPT: - printf("[TxRxResult] Incorrect status packet!\n"); - break; + return "[TxRxResult] Incorrect status packet!"; case COMM_NOT_AVAILABLE: - printf("[TxRxResult] Protocol does not support This function!\n"); - break; + return "[TxRxResult] Protocol does not support This function!"; default: - break; + return ""; } } -void Protocol2PacketHandler::printRxPacketError(uint8_t error) +void Protocol2PacketHandler::printTxRxResult(int result) +{ + printf("%s\n", getTxRxResult(result)); +} + +const char *Protocol2PacketHandler::getRxPacketError(uint8_t error) { if (error & ERRBIT_ALERT) - printf("[RxPacketError] Hardware error occurred. Check the error at Control Table (Hardware Error Status)!\n"); + return "[RxPacketError] Hardware error occurred. Check the error at Control Table (Hardware Error Status)!"; int not_alert_error = error & ~ERRBIT_ALERT; switch(not_alert_error) { case 0: - break; + return ""; case ERRNUM_RESULT_FAIL: - printf("[RxPacketError] Failed to process the instruction packet!\n"); - break; + return "[RxPacketError] Failed to process the instruction packet!"; case ERRNUM_INSTRUCTION: - printf("[RxPacketError] Undefined instruction or incorrect instruction!\n"); - break; + return "[RxPacketError] Undefined instruction or incorrect instruction!"; case ERRNUM_CRC: - printf("[RxPacketError] CRC doesn't match!\n"); - break; + return "[RxPacketError] CRC doesn't match!"; case ERRNUM_DATA_RANGE: - printf("[RxPacketError] The data value is out of range!\n"); - break; + return "[RxPacketError] The data value is out of range!"; case ERRNUM_DATA_LENGTH: - printf("[RxPacketError] The data length does not match as expected!\n"); - break; + return "[RxPacketError] The data length does not match as expected!"; case ERRNUM_DATA_LIMIT: - printf("[RxPacketError] The data value exceeds the limit value!\n"); - break; + return "[RxPacketError] The data value exceeds the limit value!"; case ERRNUM_ACCESS: - printf("[RxPacketError] Writing or Reading is not available to target address!\n"); - break; + return "[RxPacketError] Writing or Reading is not available to target address!"; default: - printf("[RxPacketError] Unknown error code!\n"); - break; + return "[RxPacketError] Unknown error code!"; } } +void Protocol2PacketHandler::printRxPacketError(uint8_t error) +{ + printf("%s\n", getRxPacketError(error)); +} + unsigned short Protocol2PacketHandler::updateCRC(uint16_t crc_accum, uint8_t *data_blk_ptr, uint16_t data_blk_size) { uint16_t i;