diff --git a/README.md b/README.md new file mode 100644 index 0000000000000000000000000000000000000000..8ccc577ba8d283840b75240711ca77aa3ad26047 --- /dev/null +++ b/README.md @@ -0,0 +1,11 @@ +### DynamixelSDK +All in one SDK for ROBOTIS Dynamixel + +* Protocol 1.0 / Protocol 2.0 + +* Windows / Linux + +-------------------------------------------------------------------------- + +###Any questions are welcomed. Just let us know through: [ISSUES](https://github.com/ROBOTIS-GIT/DynamixelSDK/issues) + diff --git a/c++/src/dynamixel_sdk/Protocol1PacketHandler.cpp b/c++/src/dynamixel_sdk/Protocol1PacketHandler.cpp index 4004f457d3c88322214977aab8cbffcfbe62a65c..66877e7fec27361b65dfbded40492ca349ffb27f 100644 --- a/c++/src/dynamixel_sdk/Protocol1PacketHandler.cpp +++ b/c++/src/dynamixel_sdk/Protocol1PacketHandler.cpp @@ -183,7 +183,12 @@ int Protocol1PacketHandler::RxPacket(PortHandler *port, UINT8_T *rxpacket) } // re-calculate the exact length of the rx packet - _wait_length = rxpacket[PKT_LENGTH] + PKT_LENGTH + 1; + if(_wait_length != rxpacket[PKT_LENGTH] + PKT_LENGTH + 1) + { + _wait_length = rxpacket[PKT_LENGTH] + PKT_LENGTH + 1; + continue; + } + if(_rx_length < _wait_length) { // check timeout