diff --git a/c++/build/win32/output/dxl_x86_cpp.dll b/c++/build/win32/output/dxl_x86_cpp.dll index 0dd9ab84..800e5724 100644 Binary files a/c++/build/win32/output/dxl_x86_cpp.dll and b/c++/build/win32/output/dxl_x86_cpp.dll differ diff --git a/c++/build/win32/output/dxl_x86_cpp.lib b/c++/build/win32/output/dxl_x86_cpp.lib index fe0569f8..c72f5eee 100644 Binary files a/c++/build/win32/output/dxl_x86_cpp.lib and b/c++/build/win32/output/dxl_x86_cpp.lib differ diff --git a/c++/build/win64/output/dxl_x64_cpp.dll b/c++/build/win64/output/dxl_x64_cpp.dll index b3b4301b..baf52443 100644 Binary files a/c++/build/win64/output/dxl_x64_cpp.dll and b/c++/build/win64/output/dxl_x64_cpp.dll differ diff --git a/c++/build/win64/output/dxl_x64_cpp.lib b/c++/build/win64/output/dxl_x64_cpp.lib index e431a9ae..18d8fcbe 100644 Binary files a/c++/build/win64/output/dxl_x64_cpp.lib and b/c++/build/win64/output/dxl_x64_cpp.lib differ diff --git a/c++/include/dynamixel_sdk/packet_handler.h b/c++/include/dynamixel_sdk/packet_handler.h index fdb4a9d6..f3b7e4fb 100644 --- a/c++/include/dynamixel_sdk/packet_handler.h +++ b/c++/include/dynamixel_sdk/packet_handler.h @@ -224,10 +224,11 @@ class WINDECLSPEC PacketHandler /// @description transmits the packet with PacketHandler::txRxPacket(), /// @description then Dynamixel reboots. /// @description During reboot, its LED will blink. + /// @description It will take some time to reboot. It will take longer for Y series. /// @param port PortHandler instance /// @param id Dynamixel ID /// @param error Dynamixel hardware error - /// @return COMM_NOT_AVAILABLE + /// @return communication results which come from PacketHandler::txRxPacket() //////////////////////////////////////////////////////////////////////////////// virtual int reboot (PortHandler *port, uint8_t id, uint8_t *error = 0) = 0; @@ -236,7 +237,8 @@ class WINDECLSPEC PacketHandler /// @description The function makes an instruction packet with INST_CLEAR, /// @description transmits the packet with PacketHandler::txRxPacket(). /// @description Applied Products : MX with Protocol 2.0 (Firmware v42 or above), - /// @description Dynamixel X-series (Firmware v42 or above). + /// @description DYNAMIXEL X-series (Firmware v42 or above), DYNAMIXEL Y-series. + /// @description It will take some time to clear. It will take longer for Y series. /// @param port PortHandler instance /// @param id Dynamixel ID /// @param error Dynamixel hardware error @@ -244,6 +246,18 @@ class WINDECLSPEC PacketHandler //////////////////////////////////////////////////////////////////////////////// virtual int clearMultiTurn (PortHandler *port, uint8_t id, uint8_t *error = 0) = 0; + //////////////////////////////////////////////////////////////////////////////// + /// @brief The function that clear errors that occurred in DYNAMIXEL + /// @description The function makes an instruction packet with INST_CLEAR, + /// @description transmits the packet with PacketHandler::txRxPacket(). + /// @description Applied Products : DYNAMIXEL Y-series. + /// @param port PortHandler instance + /// @param id DYNAMIXEL ID + /// @param error DYNAMIXEL hardware error + /// @return communication results which come from PacketHandler::txRxPacket() + //////////////////////////////////////////////////////////////////////////////// + virtual int clearError (PortHandler *port, uint8_t id, uint8_t *error = 0) = 0; + //////////////////////////////////////////////////////////////////////////////// /// @brief The function that makes Dynamixel reset as it was produced in the factory /// @description The function makes an instruction packet with INST_FACTORY_RESET, diff --git a/c++/include/dynamixel_sdk/protocol1_packet_handler.h b/c++/include/dynamixel_sdk/protocol1_packet_handler.h index 0502b63f..5fb6e997 100644 --- a/c++/include/dynamixel_sdk/protocol1_packet_handler.h +++ b/c++/include/dynamixel_sdk/protocol1_packet_handler.h @@ -187,6 +187,15 @@ class WINDECLSPEC Protocol1PacketHandler : public PacketHandler //////////////////////////////////////////////////////////////////////////////// int clearMultiTurn (PortHandler *port, uint8_t id, uint8_t *error = 0); + //////////////////////////////////////////////////////////////////////////////// + /// @brief (Available only in Protocol 2.0) The function that clear errors that occurred in DYNAMIXEL + /// @param port PortHandler instance + /// @param id DYNAMIXEL ID + /// @param error DYNAMIXEL hardware error + /// @return COMM_NOT_AVAILABLE + //////////////////////////////////////////////////////////////////////////////// + int clearError (PortHandler *port, uint8_t id, uint8_t *error = 0); + //////////////////////////////////////////////////////////////////////////////// /// @brief The function that makes Dynamixel reset as it was produced in the factory /// @description The function makes an instruction packet with INST_FACTORY_RESET, diff --git a/c++/include/dynamixel_sdk/protocol2_packet_handler.h b/c++/include/dynamixel_sdk/protocol2_packet_handler.h index 0fffed71..b1e747df 100644 --- a/c++/include/dynamixel_sdk/protocol2_packet_handler.h +++ b/c++/include/dynamixel_sdk/protocol2_packet_handler.h @@ -179,10 +179,11 @@ class WINDECLSPEC Protocol2PacketHandler : public PacketHandler /// @description transmits the packet with Protocol2PacketHandler::txRxPacket(), /// @description then Dynamixel reboots. /// @description During reboot, its LED will blink. + /// @description It will take some time to reboot. It will take longer for Y series. /// @param port PortHandler instance /// @param id Dynamixel ID /// @param error Dynamixel hardware error - /// @return COMM_NOT_AVAILABLE + /// @return communication results which come from PacketHandler::txRxPacket() //////////////////////////////////////////////////////////////////////////////// int reboot (PortHandler *port, uint8_t id, uint8_t *error = 0); @@ -190,8 +191,8 @@ class WINDECLSPEC Protocol2PacketHandler : public PacketHandler /// @brief The function that reset multi-turn revolution information of Dynamixel /// @description The function makes an instruction packet with INST_CLEAR, /// @description transmits the packet with Protocol2PacketHandler::txRxPacket(). - /// @description Applied Products : MX with Protocol 2.0 (Firmware v42 or above), - /// @description Dynamixel X-series (Firmware v42 or above). + /// @description DYNAMIXEL X-series (Firmware v42 or above), DYNAMIXEL Y-series. + /// @description It will take some time to clear. It will take longer for Y series. /// @param port PortHandler instance /// @param id Dynamixel ID /// @param error Dynamixel hardware error @@ -199,6 +200,18 @@ class WINDECLSPEC Protocol2PacketHandler : public PacketHandler //////////////////////////////////////////////////////////////////////////////// int clearMultiTurn (PortHandler *port, uint8_t id, uint8_t *error = 0); + //////////////////////////////////////////////////////////////////////////////// + /// @brief The function that clear errors that occurred in DYNAMIXEL + /// @description The function makes an instruction packet with INST_CLEAR, + /// @description transmits the packet with PacketHandler::txRxPacket(). + /// @description Applied Products : DYNAMIXEL Y-series. + /// @param port PortHandler instance + /// @param id DYNAMIXEL ID + /// @param error DYNAMIXEL hardware error + /// @return communication results which come from PacketHandler::txRxPacket() + //////////////////////////////////////////////////////////////////////////////// + int clearError (PortHandler *port, uint8_t id, uint8_t *error = 0); + //////////////////////////////////////////////////////////////////////////////// /// @brief The function that makes Dynamixel reset as it was produced in the factory /// @description The function makes an instruction packet with INST_FACTORY_RESET, diff --git a/c++/src/dynamixel_sdk/protocol1_packet_handler.cpp b/c++/src/dynamixel_sdk/protocol1_packet_handler.cpp index 51d765d3..84907584 100644 --- a/c++/src/dynamixel_sdk/protocol1_packet_handler.cpp +++ b/c++/src/dynamixel_sdk/protocol1_packet_handler.cpp @@ -370,6 +370,11 @@ int Protocol1PacketHandler::clearMultiTurn(PortHandler *port, uint8_t id, uint8_ return COMM_NOT_AVAILABLE; } +int Protocol1PacketHandler::clearError(PortHandler *port, uint8_t id, uint8_t *error) +{ + return COMM_NOT_AVAILABLE; +} + int Protocol1PacketHandler::factoryReset(PortHandler *port, uint8_t id, uint8_t option, uint8_t *error) { uint8_t txpacket[6] = {0}; diff --git a/c++/src/dynamixel_sdk/protocol2_packet_handler.cpp b/c++/src/dynamixel_sdk/protocol2_packet_handler.cpp index cc4e6f63..ef48bd18 100644 --- a/c++/src/dynamixel_sdk/protocol2_packet_handler.cpp +++ b/c++/src/dynamixel_sdk/protocol2_packet_handler.cpp @@ -446,6 +446,10 @@ int Protocol2PacketHandler::txRxPacket(PortHandler *port, uint8_t *txpacket, uin { port->setPacketTimeout((uint16_t)(DXL_MAKEWORD(txpacket[PKT_PARAMETER0+2], txpacket[PKT_PARAMETER0+3]) + 11)); } + else if (txpacket[PKT_INSTRUCTION] == INST_CLEAR) + { + port->setPacketTimeout((double)10000); + } else { port->setPacketTimeout((uint16_t)11); @@ -633,6 +637,24 @@ int Protocol2PacketHandler::clearMultiTurn(PortHandler *port, uint8_t id, uint8_ return txRxPacket(port, txpacket, rxpacket, error); } +int Protocol2PacketHandler::clearError(PortHandler *port, uint8_t id, uint8_t *error) +{ + uint8_t txpacket[15] = {0}; + uint8_t rxpacket[11] = {0}; + + txpacket[PKT_ID] = id; + txpacket[PKT_LENGTH_L] = 8; + txpacket[PKT_LENGTH_H] = 0; + txpacket[PKT_INSTRUCTION] = INST_CLEAR; + txpacket[PKT_PARAMETER0] = 0x02; + txpacket[PKT_PARAMETER0+1] = 0x45; + txpacket[PKT_PARAMETER0+2] = 0x52; + txpacket[PKT_PARAMETER0+3] = 0x43; + txpacket[PKT_PARAMETER0+4] = 0x4C; + + return txRxPacket(port, txpacket, rxpacket, error); +} + int Protocol2PacketHandler::factoryReset(PortHandler *port, uint8_t id, uint8_t option, uint8_t *error) { uint8_t txpacket[11] = {0};