Merge pull request ROBOTIS-GIT#630 from honghyun79/master

Clear Error Implementation

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,14 +237,27 @@ 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
   /// @return communication results which come from PacketHandler::txRxPacket()
   ////////////////////////////////////////////////////////////////////////////////
   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

@@ -179,26 +179,39 @@ 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);
 
   ////////////////////////////////////////////////////////////////////////////////
   /// @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
   /// @return communication results which come from Protocol2PacketHandler::txRxPacket()
   ////////////////////////////////////////////////////////////////////////////////
   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,

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