- modified "ros" directory C++ code.

Author: ROBOTIS-zerom

ros/include/dynamixel_sdk/packet_handler.h

@@ -52,6 +52,7 @@
 #define INST_BULK_READ          146     // 0x92
 // --- Only for 2.0 --- //
 #define INST_REBOOT             8
+#define INST_CLEAR              16      // 0x10
 #define INST_STATUS             85      // 0x55
 #define INST_SYNC_READ          130     // 0x82
 #define INST_BULK_WRITE         147     // 0x93
@@ -222,6 +223,19 @@ class WINDECLSPEC PacketHandler
   ////////////////////////////////////////////////////////////////////////////////
   virtual int reboot          (PortHandler *port, uint8_t id, uint8_t *error = 0) = 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 PacketHandler::txRxPacket().
+  /// @description Applied Products : MX with Protocol 2.0 (Firmware v42 or above),
+  /// @description Dynamixel X-series (Firmware v42 or above).
+  /// @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 makes Dynamixel reset as it was produced in the factory
   /// @description The function makes an instruction packet with INST_FACTORY_RESET,

ros/include/dynamixel_sdk/protocol1_packet_handler.h

@@ -178,6 +178,15 @@ class WINDECLSPEC Protocol1PacketHandler : public PacketHandler
   ////////////////////////////////////////////////////////////////////////////////
   int reboot          (PortHandler *port, uint8_t id, uint8_t *error = 0);
 
+  ////////////////////////////////////////////////////////////////////////////////
+  /// @brief (Available only in Protocol 2.0) The function that reset multi-turn revolution information of Dynamixel
+  /// @param port PortHandler instance
+  /// @param id Dynamixel ID
+  /// @param error Dynamixel hardware error
+  /// @return COMM_NOT_AVAILABLE
+  ////////////////////////////////////////////////////////////////////////////////
+  int clearMultiTurn  (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,

ros/include/dynamixel_sdk/protocol2_packet_handler.h

@@ -186,6 +186,19 @@ class WINDECLSPEC Protocol2PacketHandler : public PacketHandler
   ////////////////////////////////////////////////////////////////////////////////
   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).
+  /// @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 makes Dynamixel reset as it was produced in the factory
   /// @description The function makes an instruction packet with INST_FACTORY_RESET,

ros/src/dynamixel_sdk/protocol1_packet_handler.cpp

@@ -365,6 +365,11 @@ int Protocol1PacketHandler::reboot(PortHandler *port, uint8_t id, uint8_t *error
   return COMM_NOT_AVAILABLE;
 }
 
+int Protocol1PacketHandler::clearMultiTurn(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};

ros/src/dynamixel_sdk/protocol2_packet_handler.cpp

@@ -612,6 +612,24 @@ int Protocol2PacketHandler::reboot(PortHandler *port, uint8_t id, uint8_t *error
   return txRxPacket(port, txpacket, rxpacket, error);
 }
 
+int Protocol2PacketHandler::clearMultiTurn(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]    = 0x01;
+  txpacket[PKT_PARAMETER0+1]  = 0x44;
+  txpacket[PKT_PARAMETER0+2]  = 0x58;
+  txpacket[PKT_PARAMETER0+3]  = 0x4C;
+  txpacket[PKT_PARAMETER0+4]  = 0x22;
+
+  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};