add clearError

Author: honghyun79

c++/include/dynamixel_sdk/packet_handler.h

@@ -246,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,

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,

c++/include/dynamixel_sdk/protocol2_packet_handler.h

@@ -200,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) = 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,

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};

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};