- renewal

2016-03-04 21:01:35 +09:00
parent fe3b64d56a
commit 587ba84c70
58 changed files with 6452 additions and 0 deletions
--- a/dynamixel_sdk/CMakeLists.txt
+++ b/dynamixel_sdk/CMakeLists.txt
@@ -0,0 +1,38 @@
+cmake_minimum_required(VERSION 2.8.3)
+project(dynamixel_sdk)
+
+find_package(catkin REQUIRED COMPONENTS
+  roscpp
+)
+
+catkin_package(
+  INCLUDE_DIRS include
+#  LIBRARIES dynamixel_sdk
+#  CATKIN_DEPENDS roscpp
+#  DEPENDS system_lib
+)
+
+# include_directories(include)
+include_directories(
+  include
+  ${catkin_INCLUDE_DIRS}
+)
+
+## Declare a C++ library
+add_library(dynamixel_sdk
+  src/${PROJECT_NAME}/PacketHandler.cpp
+  src/${PROJECT_NAME}/Protocol1PacketHandler.cpp
+  src/${PROJECT_NAME}/Protocol2PacketHandler.cpp
+  src/${PROJECT_NAME}/GroupSyncRead.cpp
+  src/${PROJECT_NAME}/GroupSyncWrite.cpp
+  src/${PROJECT_NAME}/GroupBulkRead.cpp
+  src/${PROJECT_NAME}/GroupBulkWrite.cpp
+  src/${PROJECT_NAME}/PortHandler.cpp
+  src/${PROJECT_NAME}_linux/PortHandlerLinux.cpp
+)
+
+## Specify libraries to link a library or executable target against
+target_link_libraries(dynamixel_sdk
+  ${catkin_LIBRARIES}
+)
+
--- a/dynamixel_sdk/include/dynamixel_sdk/GroupBulkRead.h
+++ b/dynamixel_sdk/include/dynamixel_sdk/GroupBulkRead.h
@@ -0,0 +1,59 @@
+/*
+ * GroupBulkRead.h
+ *
+ *  Created on: 2016. 1. 28.
+ *      Author: zerom
+ */
+
+#ifndef ROBOTIS_FRAMEWORK_DYNAMIXEL_SDK_INCLUDE_DYNAMIXEL_SDK_GROUPBULKREAD_H_
+#define ROBOTIS_FRAMEWORK_DYNAMIXEL_SDK_INCLUDE_DYNAMIXEL_SDK_GROUPBULKREAD_H_
+
+
+#include <map>
+#include <vector>
+#include "RobotisDef.h"
+#include "PortHandler.h"
+#include "PacketHandler.h"
+
+namespace ROBOTIS
+{
+
+class GroupBulkRead
+{
+private:
+    PortHandler     *port_;
+    PacketHandler   *ph_;
+
+    std::vector<UINT8_T>            id_list_;
+    std::map<UINT8_T, UINT16_T>     address_list_;  // <id, start_address>
+    std::map<UINT8_T, UINT16_T>     length_list_;   // <id, data_length>
+    std::map<UINT8_T, UINT8_T *>    data_list_;     // <id, data>
+
+    UINT8_T         *param_;
+
+    void    MakeParam();
+
+public:
+    GroupBulkRead(PortHandler *port, PacketHandler *ph);
+    ~GroupBulkRead() { ClearParam(); }
+
+    PortHandler     *GetPortHandler()   { return port_; }
+    PacketHandler   *GetPacketHandler() { return ph_; }
+
+    bool    AddParam    (UINT8_T id, UINT16_T start_address, UINT16_T data_length);
+    void    RemoveParam (UINT8_T id);
+    void    ClearParam  ();
+
+    int     TxPacket();
+    int     RxPacket();
+    int     TxRxPacket();
+
+    bool    GetData(UINT8_T id, UINT16_T address, UINT8_T *data);
+    bool    GetData(UINT8_T id, UINT16_T address, UINT16_T *data);
+    bool    GetData(UINT8_T id, UINT16_T address, UINT32_T *data);
+};
+
+}
+
+
+#endif /* ROBOTIS_FRAMEWORK_DYNAMIXEL_SDK_INCLUDE_DYNAMIXEL_SDK_GROUPBULKREAD_H_ */
--- a/dynamixel_sdk/include/dynamixel_sdk/GroupBulkWrite.h
+++ b/dynamixel_sdk/include/dynamixel_sdk/GroupBulkWrite.h
@@ -0,0 +1,55 @@
+/*
+ * GroupBulkWrite.h
+ *
+ *  Created on: 2016. 2. 2.
+ *      Author: zerom
+ */
+
+#ifndef ROBOTIS_FRAMEWORK_DYNAMIXEL_SDK_INCLUDE_DYNAMIXEL_SDK_GROUPBULKWRITE_H_
+#define ROBOTIS_FRAMEWORK_DYNAMIXEL_SDK_INCLUDE_DYNAMIXEL_SDK_GROUPBULKWRITE_H_
+
+
+#include <map>
+#include <vector>
+#include "RobotisDef.h"
+#include "PortHandler.h"
+#include "PacketHandler.h"
+
+namespace ROBOTIS
+{
+
+class GroupBulkWrite
+{
+private:
+    PortHandler    *port_;
+    PacketHandler  *ph_;
+
+    std::vector<UINT8_T>            id_list_;
+    std::map<UINT8_T, UINT16_T>     address_list_;  // <id, start_address>
+    std::map<UINT8_T, UINT16_T>     length_list_;   // <id, data_length>
+    std::map<UINT8_T, UINT8_T *>    data_list_;     // <id, data>
+
+    UINT8_T        *param_;
+    UINT16_T        param_length_;
+
+    void    MakeParam();
+
+public:
+    GroupBulkWrite(PortHandler *port, PacketHandler *ph);
+    ~GroupBulkWrite() { ClearParam(); }
+
+    PortHandler     *GetPortHandler()   { return port_; }
+    PacketHandler   *GetPacketHandler() { return ph_; }
+
+    bool    AddParam    (UINT8_T id, UINT16_T start_address, UINT16_T data_length, UINT8_T *data);
+    void    RemoveParam (UINT8_T id);
+    bool    ChangeParam (UINT8_T id, UINT16_T start_address, UINT16_T data_length, UINT8_T *data);
+    void    ClearParam  ();
+
+    int     TxPacket();
+};
+
+}
+
+
+#endif /* ROBOTIS_FRAMEWORK_DYNAMIXEL_SDK_INCLUDE_DYNAMIXEL_SDK_GROUPBULKWRITE_H_ */
--- a/dynamixel_sdk/include/dynamixel_sdk/GroupSyncRead.h
+++ b/dynamixel_sdk/include/dynamixel_sdk/GroupSyncRead.h
@@ -0,0 +1,59 @@
+/*
+ * GroupSyncRead.h
+ *
+ *  Created on: 2016. 2. 2.
+ *      Author: zerom
+ */
+
+#ifndef ROBOTIS_FRAMEWORK_DYNAMIXEL_SDK_INCLUDE_DYNAMIXEL_SDK_GROUPSYNCREAD_H_
+#define ROBOTIS_FRAMEWORK_DYNAMIXEL_SDK_INCLUDE_DYNAMIXEL_SDK_GROUPSYNCREAD_H_
+
+
+#include <map>
+#include <vector>
+#include "RobotisDef.h"
+#include "PortHandler.h"
+#include "PacketHandler.h"
+
+namespace ROBOTIS
+{
+
+class GroupSyncRead
+{
+private:
+    PortHandler    *port_;
+    PacketHandler  *ph_;
+
+    std::vector<UINT8_T>            id_list_;
+    std::map<UINT8_T, UINT8_T* >    data_list_; // <id, data>
+
+    UINT8_T        *param_;
+    UINT16_T        start_address_;
+    UINT16_T        data_length_;
+
+    void    MakeParam();
+
+public:
+    GroupSyncRead(PortHandler *port, PacketHandler *ph, UINT16_T start_address, UINT16_T data_length);
+    ~GroupSyncRead() { ClearParam(); }
+
+    PortHandler     *GetPortHandler()   { return port_; }
+    PacketHandler   *GetPacketHandler() { return ph_; }
+
+    bool    AddParam    (UINT8_T id);
+    void    RemoveParam (UINT8_T id);
+    void    ClearParam  ();
+
+    int     TxPacket();
+    int     RxPacket();
+    int     TxRxPacket();
+
+    bool    GetData(UINT8_T id, UINT16_T address, UINT8_T *data);
+    bool    GetData(UINT8_T id, UINT16_T address, UINT16_T *data);
+    bool    GetData(UINT8_T id, UINT16_T address, UINT32_T *data);
+};
+
+}
+
+
+#endif /* ROBOTIS_FRAMEWORK_DYNAMIXEL_SDK_INCLUDE_DYNAMIXEL_SDK_GROUPSYNCREAD_H_ */
--- a/dynamixel_sdk/include/dynamixel_sdk/GroupSyncWrite.h
+++ b/dynamixel_sdk/include/dynamixel_sdk/GroupSyncWrite.h
@@ -0,0 +1,54 @@
+/*
+ * GroupSyncWrite.h
+ *
+ *  Created on: 2016. 1. 28.
+ *      Author: zerom
+ */
+
+#ifndef ROBOTIS_FRAMEWORK_DYNAMIXEL_SDK_INCLUDE_DYNAMIXEL_SDK_GROUPSYNCWRITE_H_
+#define ROBOTIS_FRAMEWORK_DYNAMIXEL_SDK_INCLUDE_DYNAMIXEL_SDK_GROUPSYNCWRITE_H_
+
+
+#include <map>
+#include <vector>
+#include "RobotisDef.h"
+#include "PortHandler.h"
+#include "PacketHandler.h"
+
+namespace ROBOTIS
+{
+
+class GroupSyncWrite
+{
+private:
+    PortHandler    *port_;
+    PacketHandler  *ph_;
+
+    std::vector<UINT8_T>            id_list_;
+    std::map<UINT8_T, UINT8_T* >    data_list_; // <id, data>
+
+    UINT8_T        *param_;
+    UINT16_T        start_address_;
+    UINT16_T        data_length_;
+
+    void    MakeParam();
+
+public:
+    GroupSyncWrite(PortHandler *port, PacketHandler *ph, UINT16_T start_address, UINT16_T data_length);
+    ~GroupSyncWrite() { ClearParam(); }
+
+    PortHandler     *GetPortHandler()   { return port_; }
+    PacketHandler   *GetPacketHandler() { return ph_; }
+
+    bool    AddParam    (UINT8_T id, UINT8_T *data);
+    void    RemoveParam (UINT8_T id);
+    bool    ChangeParam (UINT8_T id, UINT8_T *data);
+    void    ClearParam  ();
+
+    int     TxPacket();
+};
+
+}
+
+
+#endif /* ROBOTIS_FRAMEWORK_DYNAMIXEL_SDK_INCLUDE_DYNAMIXEL_SDK_GROUPSYNCWRITE_H_ */
--- a/dynamixel_sdk/include/dynamixel_sdk/PacketHandler.h
+++ b/dynamixel_sdk/include/dynamixel_sdk/PacketHandler.h
@@ -0,0 +1,130 @@
+/*
+ * PacketHandler.h
+ *
+ *  Created on: 2016. 1. 26.
+ *      Author: zerom
+ */
+
+#ifndef ROBOTIS_FRAMEWORK_DYNAMIXEL_SDK_INCLUDE_DYNAMIXEL_SDK_PACKETHANDLER_H_
+#define ROBOTIS_FRAMEWORK_DYNAMIXEL_SDK_INCLUDE_DYNAMIXEL_SDK_PACKETHANDLER_H_
+
+
+#include <vector>
+#include "RobotisDef.h"
+#include "PortHandler.h"
+
+#define BROADCAST_ID        0xFE    // 254
+#define MAX_ID              0xFC    // 252
+
+/* Macro for Control Table Value */
+#define DXL_MAKEWORD(a, b)  ((unsigned short)(((unsigned char)(((unsigned long)(a)) & 0xff)) | ((unsigned short)((unsigned char)(((unsigned long)(b)) & 0xff))) << 8))
+#define DXL_MAKEDWORD(a, b) ((unsigned int)(((unsigned short)(((unsigned long)(a)) & 0xffff)) | ((unsigned int)((unsigned short)(((unsigned long)(b)) & 0xffff))) << 16))
+#define DXL_LOWORD(l)       ((unsigned short)(((unsigned long)(l)) & 0xffff))
+#define DXL_HIWORD(l)       ((unsigned short)((((unsigned long)(l)) >> 16) & 0xffff))
+#define DXL_LOBYTE(w)       ((unsigned char)(((unsigned long)(w)) & 0xff))
+#define DXL_HIBYTE(w)       ((unsigned char)((((unsigned long)(w)) >> 8) & 0xff))
+
+/* Instruction for DXL Protocol */
+#define INST_PING               1
+#define INST_READ               2
+#define INST_WRITE              3
+#define INST_REG_WRITE          4
+#define INST_ACTION             5
+#define INST_FACTORY_RESET      6
+#define INST_SYNC_WRITE         131     // 0x83
+#define INST_BULK_READ          146     // 0x92
+// --- Only for 2.0 --- //
+#define INST_REBOOT             8
+#define INST_STATUS             85      // 0x55
+#define INST_SYNC_READ          130     // 0x82
+#define INST_BULK_WRITE         147     // 0x93
+
+// Communication Result
+#define COMM_SUCCESS        0       // tx or rx packet communication success
+#define COMM_PORT_BUSY      -1000   // Port is busy (in use)
+#define COMM_TX_FAIL        -1001   // Failed transmit instruction packet
+#define COMM_RX_FAIL        -1002   // Failed get status packet
+#define COMM_TX_ERROR       -2000   // Incorrect instruction packet
+#define COMM_RX_WAITING     -3000   // Now recieving status packet
+#define COMM_RX_TIMEOUT     -3001   // There is no status packet
+#define COMM_RX_CORRUPT     -3002   // Incorrect status packet
+#define COMM_NOT_AVAILABLE  -9000   //
+
+namespace ROBOTIS
+{
+
+class PacketHandler
+{
+protected:
+    PacketHandler() { }
+
+public:
+    static PacketHandler *GetPacketHandler(float protocol_version = 2.0);
+
+    virtual ~PacketHandler() { }
+
+    virtual float   GetProtocolVersion() = 0;
+
+    virtual int TxPacket        (PortHandler *port, UINT8_T *txpacket) = 0;
+    virtual int RxPacket        (PortHandler *port, UINT8_T *rxpacket) = 0;
+    virtual int TxRxPacket      (PortHandler *port, UINT8_T *txpacket, UINT8_T *rxpacket, UINT8_T *error = 0) = 0;
+
+    virtual int Ping            (PortHandler *port, UINT8_T id, UINT8_T *error = 0) = 0;
+    virtual int Ping            (PortHandler *port, UINT8_T id, UINT16_T *model_number, UINT8_T *error = 0) = 0;
+
+    // BroadcastPing
+    virtual int BroadcastPing   (PortHandler *port, std::vector<UINT8_T> &id_list) = 0;
+
+    virtual int Action          (PortHandler *port, UINT8_T id) = 0;
+    virtual int Reboot          (PortHandler *port, UINT8_T id, UINT8_T *error = 0) = 0;
+    virtual int FactoryReset    (PortHandler *port, UINT8_T id, UINT8_T option = 0, UINT8_T *error = 0) = 0;
+
+
+    virtual int ReadTx          (PortHandler *port, UINT8_T id, UINT16_T address, UINT16_T length) = 0;
+    virtual int ReadRx          (PortHandler *port, UINT16_T length, UINT8_T *data, UINT8_T *error = 0) = 0;
+    virtual int ReadTxRx        (PortHandler *port, UINT8_T id, UINT16_T address, UINT16_T length, UINT8_T *data, UINT8_T *error = 0) = 0;
+
+    virtual int Read1ByteTx     (PortHandler *port, UINT8_T id, UINT16_T address) = 0;
+    virtual int Read1ByteRx     (PortHandler *port, UINT8_T *data, UINT8_T *error = 0) = 0;
+    virtual int Read1ByteTxRx   (PortHandler *port, UINT8_T id, UINT16_T address, UINT8_T *data, UINT8_T *error = 0) = 0;
+
+    virtual int Read2ByteTx     (PortHandler *port, UINT8_T id, UINT16_T address) = 0;
+    virtual int Read2ByteRx     (PortHandler *port, UINT16_T *data, UINT8_T *error = 0) = 0;
+    virtual int Read2ByteTxRx   (PortHandler *port, UINT8_T id, UINT16_T address, UINT16_T *data, UINT8_T *error = 0) = 0;
+
+    virtual int Read4ByteTx     (PortHandler *port, UINT8_T id, UINT16_T address) = 0;
+    virtual int Read4ByteRx     (PortHandler *port, UINT32_T *data, UINT8_T *error = 0) = 0;
+    virtual int Read4ByteTxRx   (PortHandler *port, UINT8_T id, UINT16_T address, UINT32_T *data, UINT8_T *error = 0) = 0;
+
+    virtual int WriteTxOnly     (PortHandler *port, UINT8_T id, UINT16_T address, UINT16_T length, UINT8_T *data) = 0;
+    virtual int WriteTxRx       (PortHandler *port, UINT8_T id, UINT16_T address, UINT16_T length, UINT8_T *data, UINT8_T *error = 0) = 0;
+
+    virtual int Write1ByteTxOnly(PortHandler *port, UINT8_T id, UINT16_T address, UINT8_T data) = 0;
+    virtual int Write1ByteTxRx  (PortHandler *port, UINT8_T id, UINT16_T address, UINT8_T data, UINT8_T *error = 0) = 0;
+
+    virtual int Write2ByteTxOnly(PortHandler *port, UINT8_T id, UINT16_T address, UINT16_T data) = 0;
+    virtual int Write2ByteTxRx  (PortHandler *port, UINT8_T id, UINT16_T address, UINT16_T data, UINT8_T *error = 0) = 0;
+
+    virtual int Write4ByteTxOnly(PortHandler *port, UINT8_T id, UINT16_T address, UINT32_T data) = 0;
+    virtual int Write4ByteTxRx  (PortHandler *port, UINT8_T id, UINT16_T address, UINT32_T data, UINT8_T *error = 0) = 0;
+
+    virtual int RegWriteTxOnly  (PortHandler *port, UINT8_T id, UINT16_T address, UINT16_T length, UINT8_T *data) = 0;
+    virtual int RegWriteTxRx    (PortHandler *port, UINT8_T id, UINT16_T address, UINT16_T length, UINT8_T *data, UINT8_T *error = 0) = 0;
+
+    virtual int SyncReadTx      (PortHandler *port, UINT16_T start_address, UINT16_T data_length, UINT8_T *param, UINT16_T param_length) = 0;
+    // SyncReadRx   -> GroupSyncRead class
+    // SyncReadTxRx -> GroupSyncRead class
+
+    virtual int SyncWriteTxOnly (PortHandler *port, UINT16_T start_address, UINT16_T data_length, UINT8_T *param, UINT16_T param_length) = 0;
+
+    virtual int BulkReadTx      (PortHandler *port, UINT8_T *param, UINT16_T param_length) = 0;
+    // BulkReadRx   -> GroupBulkRead class
+    // BulkReadTxRx -> GroupBulkRead class
+
+    virtual int BulkWriteTxOnly (PortHandler *port, UINT8_T *param, UINT16_T param_length) = 0;
+};
+
+}
+
+
+#endif /* ROBOTIS_FRAMEWORK_DYNAMIXEL_SDK_INCLUDE_DYNAMIXEL_SDK_PACKETHANDLER_H_ */
--- a/dynamixel_sdk/include/dynamixel_sdk/PortHandler.h
+++ b/dynamixel_sdk/include/dynamixel_sdk/PortHandler.h
@@ -0,0 +1,51 @@
+/*
+ * PortHandler.h
+ *
+ *  Created on: 2016. 1. 26.
+ *      Author: zerom
+ */
+
+#ifndef ROBOTIS_FRAMEWORK_DYNAMIXEL_SDK_INCLUDE_DYNAMIXEL_SDK_PORTHANDLER_H_
+#define ROBOTIS_FRAMEWORK_DYNAMIXEL_SDK_INCLUDE_DYNAMIXEL_SDK_PORTHANDLER_H_
+
+
+#include "RobotisDef.h"
+
+namespace ROBOTIS
+{
+
+class PortHandler
+{
+public:
+    static const int DEFAULT_BAUDRATE = 1000000;
+
+    static PortHandler *GetPortHandler(const char *port_name);
+
+    bool    is_using;
+
+    virtual ~PortHandler() { }
+
+    virtual bool    OpenPort() = 0;
+    virtual void    ClosePort() = 0;
+    virtual void    ClearPort() = 0;
+
+    virtual void    SetPortName(const char* port_name) = 0;
+    virtual char   *GetPortName() = 0;
+
+    virtual bool    SetBaudRate(const int baudrate) = 0;
+    virtual int     GetBaudRate() = 0;
+
+    virtual int     GetBytesAvailable() = 0;
+
+    virtual int     ReadPort(UINT8_T *packet, int length) = 0;
+    virtual int     WritePort(UINT8_T *packet, int length) = 0;
+
+    virtual void    SetPacketTimeout(UINT16_T packet_length) = 0;
+    virtual void    SetPacketTimeout(double msec) = 0;
+    virtual bool    IsPacketTimeout() = 0;
+};
+
+}
+
+
+#endif /* ROBOTIS_FRAMEWORK_DYNAMIXEL_SDK_INCLUDE_DYNAMIXEL_SDK_PORTHANDLER_H_ */
--- a/dynamixel_sdk/include/dynamixel_sdk/Protocol1PacketHandler.h
+++ b/dynamixel_sdk/include/dynamixel_sdk/Protocol1PacketHandler.h
@@ -0,0 +1,95 @@
+/*
+ * Protocol1PacketHandler.h
+ *
+ *  Created on: 2016. 1. 26.
+ *      Author: zerom
+ */
+
+#ifndef ROBOTIS_FRAMEWORK_DYNAMIXEL_SDK_INCLUDE_DYNAMIXEL_SDK_PROTOCOL1PACKETHANDLER_H_
+#define ROBOTIS_FRAMEWORK_DYNAMIXEL_SDK_INCLUDE_DYNAMIXEL_SDK_PROTOCOL1PACKETHANDLER_H_
+
+
+#include "PacketHandler.h"
+
+namespace ROBOTIS
+{
+
+class Protocol1PacketHandler : public PacketHandler
+{
+private:
+    static Protocol1PacketHandler *unique_instance_;
+
+    Protocol1PacketHandler();
+
+public:
+    static Protocol1PacketHandler *GetInstance() { return unique_instance_; }
+
+    virtual ~Protocol1PacketHandler() { }
+
+    float   GetProtocolVersion() { return 1.0; }
+
+    int TxPacket        (PortHandler *port, UINT8_T *txpacket);
+    int RxPacket        (PortHandler *port, UINT8_T *rxpacket);
+    int TxRxPacket      (PortHandler *port, UINT8_T *txpacket, UINT8_T *rxpacket, UINT8_T *error = 0);
+
+    int Ping            (PortHandler *port, UINT8_T id, UINT8_T *error = 0);
+    int Ping            (PortHandler *port, UINT8_T id, UINT16_T *model_number, UINT8_T *error = 0);
+
+    // BroadcastPing
+    int BroadcastPing   (PortHandler *port, std::vector<UINT8_T> &id_list);
+
+    int Action          (PortHandler *port, UINT8_T id);
+    int Reboot          (PortHandler *port, UINT8_T id, UINT8_T *error = 0);
+    int FactoryReset    (PortHandler *port, UINT8_T id, UINT8_T option, UINT8_T *error = 0);
+
+
+    int ReadTx          (PortHandler *port, UINT8_T id, UINT16_T address, UINT16_T length);
+    int ReadRx          (PortHandler *port, UINT16_T length, UINT8_T *data, UINT8_T *error = 0);
+    int ReadTxRx        (PortHandler *port, UINT8_T id, UINT16_T address, UINT16_T length, UINT8_T *data, UINT8_T *error = 0);
+
+    int Read1ByteTx     (PortHandler *port, UINT8_T id, UINT16_T address);
+    int Read1ByteRx     (PortHandler *port, UINT8_T *data, UINT8_T *error = 0);
+    int Read1ByteTxRx       (PortHandler *port, UINT8_T id, UINT16_T address, UINT8_T *data, UINT8_T *error = 0);
+
+    int Read2ByteTx     (PortHandler *port, UINT8_T id, UINT16_T address);
+    int Read2ByteRx     (PortHandler *port, UINT16_T *data, UINT8_T *error = 0);
+    int Read2ByteTxRx       (PortHandler *port, UINT8_T id, UINT16_T address, UINT16_T *data, UINT8_T *error = 0);
+
+    int Read4ByteTx     (PortHandler *port, UINT8_T id, UINT16_T address);
+    int Read4ByteRx     (PortHandler *port, UINT32_T *data, UINT8_T *error = 0);
+    int Read4ByteTxRx       (PortHandler *port, UINT8_T id, UINT16_T address, UINT32_T *data, UINT8_T *error = 0);
+
+    int WriteTxOnly     (PortHandler *port, UINT8_T id, UINT16_T address, UINT16_T length, UINT8_T *data);
+    int WriteTxRx           (PortHandler *port, UINT8_T id, UINT16_T address, UINT16_T length, UINT8_T *data, UINT8_T *error = 0);
+
+    int Write1ByteTxOnly(PortHandler *port, UINT8_T id, UINT16_T address, UINT8_T data);
+    int Write1ByteTxRx      (PortHandler *port, UINT8_T id, UINT16_T address, UINT8_T data, UINT8_T *error = 0);
+
+    int Write2ByteTxOnly(PortHandler *port, UINT8_T id, UINT16_T address, UINT16_T data);
+    int Write2ByteTxRx      (PortHandler *port, UINT8_T id, UINT16_T address, UINT16_T data, UINT8_T *error = 0);
+
+    int Write4ByteTxOnly(PortHandler *port, UINT8_T id, UINT16_T address, UINT32_T data);
+    int Write4ByteTxRx      (PortHandler *port, UINT8_T id, UINT16_T address, UINT32_T data, UINT8_T *error = 0);
+
+    int RegWriteTxOnly  (PortHandler *port, UINT8_T id, UINT16_T address, UINT16_T length, UINT8_T *data);
+    int RegWriteTxRx        (PortHandler *port, UINT8_T id, UINT16_T address, UINT16_T length, UINT8_T *data, UINT8_T *error = 0);
+
+    int SyncReadTx      (PortHandler *port, UINT16_T start_address, UINT16_T data_length, UINT8_T *param, UINT16_T param_length);
+    // SyncReadRx   -> GroupSyncRead class
+    // SyncReadTxRx -> GroupSyncRead class
+
+    // param : ID1 DATA0 DATA1 ... DATAn ID2 DATA0 DATA1 ... DATAn ID3 DATA0 DATA1 ... DATAn
+    int SyncWriteTxOnly (PortHandler *port, UINT16_T start_address, UINT16_T data_length, UINT8_T *param, UINT16_T param_length);
+
+    // param : LEN1 ID1 ADDR1 LEN2 ID2 ADDR2 ...
+    int BulkReadTx      (PortHandler *port, UINT8_T *param, UINT16_T param_length);
+    // BulkReadRx   -> GroupBulkRead class
+    // BulkReadTxRx -> GroupBulkRead class
+
+    int BulkWriteTxOnly (PortHandler *port, UINT8_T *param, UINT16_T param_length);
+};
+
+}
+
+
+#endif /* ROBOTIS_FRAMEWORK_DYNAMIXEL_SDK_INCLUDE_DYNAMIXEL_SDK_PROTOCOL1PACKETHANDLER_H_ */
--- a/dynamixel_sdk/include/dynamixel_sdk/Protocol2PacketHandler.h
+++ b/dynamixel_sdk/include/dynamixel_sdk/Protocol2PacketHandler.h
@@ -0,0 +1,100 @@
+/*
+ * Protocol2PacketHandler.h
+ *
+ *  Created on: 2016. 1. 26.
+ *      Author: zerom
+ */
+
+#ifndef ROBOTIS_FRAMEWORK_DYNAMIXEL_SDK_INCLUDE_DYNAMIXEL_SDK_PROTOCOL2PACKETHANDLER_H_
+#define ROBOTIS_FRAMEWORK_DYNAMIXEL_SDK_INCLUDE_DYNAMIXEL_SDK_PROTOCOL2PACKETHANDLER_H_
+
+
+#include "PacketHandler.h"
+
+namespace ROBOTIS
+{
+
+class Protocol2PacketHandler : public PacketHandler
+{
+private:
+    static Protocol2PacketHandler *unique_instance_;
+
+    Protocol2PacketHandler();
+
+    UINT16_T    UpdateCRC(UINT16_T crc_accum, UINT8_T *data_blk_ptr, UINT16_T data_blk_size);
+    void        AddStuffing(UINT8_T *packet);
+    void        RemoveStuffing(UINT8_T *packet);
+
+public:
+    static Protocol2PacketHandler *GetInstance() { return unique_instance_; }
+
+    virtual ~Protocol2PacketHandler() { }
+
+    float   GetProtocolVersion() { return 2.0; }
+
+    int TxPacket        (PortHandler *port, UINT8_T *txpacket);
+    int RxPacket        (PortHandler *port, UINT8_T *rxpacket);
+    int TxRxPacket      (PortHandler *port, UINT8_T *txpacket, UINT8_T *rxpacket, UINT8_T *error = 0);
+
+    int Ping            (PortHandler *port, UINT8_T id, UINT8_T *error = 0);
+    int Ping            (PortHandler *port, UINT8_T id, UINT16_T *model_number, UINT8_T *error = 0);
+
+    // BroadcastPing
+    int BroadcastPing   (PortHandler *port, std::vector<UINT8_T> &id_list);
+
+    int Action          (PortHandler *port, UINT8_T id);
+    int Reboot          (PortHandler *port, UINT8_T id, UINT8_T *error = 0);
+    int FactoryReset    (PortHandler *port, UINT8_T id, UINT8_T option, UINT8_T *error = 0);
+
+
+    int ReadTx          (PortHandler *port, UINT8_T id, UINT16_T address, UINT16_T length);
+    int ReadRx          (PortHandler *port, UINT16_T length, UINT8_T *data, UINT8_T *error = 0);
+    int ReadTxRx        (PortHandler *port, UINT8_T id, UINT16_T address, UINT16_T length, UINT8_T *data, UINT8_T *error = 0);
+
+    int Read1ByteTx     (PortHandler *port, UINT8_T id, UINT16_T address);
+    int Read1ByteRx     (PortHandler *port, UINT8_T *data, UINT8_T *error = 0);
+    int Read1ByteTxRx       (PortHandler *port, UINT8_T id, UINT16_T address, UINT8_T *data, UINT8_T *error = 0);
+
+    int Read2ByteTx     (PortHandler *port, UINT8_T id, UINT16_T address);
+    int Read2ByteRx     (PortHandler *port, UINT16_T *data, UINT8_T *error = 0);
+    int Read2ByteTxRx       (PortHandler *port, UINT8_T id, UINT16_T address, UINT16_T *data, UINT8_T *error = 0);
+
+    int Read4ByteTx     (PortHandler *port, UINT8_T id, UINT16_T address);
+    int Read4ByteRx     (PortHandler *port, UINT32_T *data, UINT8_T *error = 0);
+    int Read4ByteTxRx       (PortHandler *port, UINT8_T id, UINT16_T address, UINT32_T *data, UINT8_T *error = 0);
+
+    int WriteTxOnly     (PortHandler *port, UINT8_T id, UINT16_T address, UINT16_T length, UINT8_T *data);
+    int WriteTxRx           (PortHandler *port, UINT8_T id, UINT16_T address, UINT16_T length, UINT8_T *data, UINT8_T *error = 0);
+
+    int Write1ByteTxOnly(PortHandler *port, UINT8_T id, UINT16_T address, UINT8_T data);
+    int Write1ByteTxRx      (PortHandler *port, UINT8_T id, UINT16_T address, UINT8_T data, UINT8_T *error = 0);
+
+    int Write2ByteTxOnly(PortHandler *port, UINT8_T id, UINT16_T address, UINT16_T data);
+    int Write2ByteTxRx      (PortHandler *port, UINT8_T id, UINT16_T address, UINT16_T data, UINT8_T *error = 0);
+
+    int Write4ByteTxOnly(PortHandler *port, UINT8_T id, UINT16_T address, UINT32_T data);
+    int Write4ByteTxRx      (PortHandler *port, UINT8_T id, UINT16_T address, UINT32_T data, UINT8_T *error = 0);
+
+    int RegWriteTxOnly  (PortHandler *port, UINT8_T id, UINT16_T address, UINT16_T length, UINT8_T *data);
+    int RegWriteTxRx        (PortHandler *port, UINT8_T id, UINT16_T address, UINT16_T length, UINT8_T *data, UINT8_T *error = 0);
+
+    int SyncReadTx      (PortHandler *port, UINT16_T start_address, UINT16_T data_length, UINT8_T *param, UINT16_T param_length);
+    // SyncReadRx   -> GroupSyncRead class
+    // SyncReadTxRx -> GroupSyncRead class
+
+    // param : ID1 DATA0 DATA1 ... DATAn ID2 DATA0 DATA1 ... DATAn ID3 DATA0 DATA1 ... DATAn
+    int SyncWriteTxOnly (PortHandler *port, UINT16_T start_address, UINT16_T data_length, UINT8_T *param, UINT16_T param_length);
+
+    // param : ID1 ADDR_L1 ADDR_H1 LEN_L1 LEN_H1 ID2 ADDR_L2 ADDR_H2 LEN_L2 LEN_H2 ...
+    int BulkReadTx      (PortHandler *port, UINT8_T *param, UINT16_T param_length);
+    // BulkReadRx   -> GroupBulkRead class
+    // BulkReadTxRx -> GroupBulkRead class
+
+    // param : ID1 START_ADDR_L START_ADDR_H DATA_LEN_L DATA_LEN_H DATA0 DATA1 ... DATAn ID2 START_ADDR_L START_ADDR_H DATA_LEN_L DATA_LEN_H DATA0 DATA1 ... DATAn
+    int BulkWriteTxOnly (PortHandler *port, UINT8_T *param, UINT16_T param_length);
+};
+
+}
+
+
+#endif /* ROBOTIS_FRAMEWORK_DYNAMIXEL_SDK_INCLUDE_DYNAMIXEL_SDK_PROTOCOL2PACKETHANDLER_H_ */
--- a/dynamixel_sdk/include/dynamixel_sdk/RobotisDef.h
+++ b/dynamixel_sdk/include/dynamixel_sdk/RobotisDef.h
@@ -0,0 +1,21 @@
+/*
+ * RobotisDef.h
+ *
+ *  Created on: 2016. 1. 27.
+ *      Author: zerom
+ */
+
+#ifndef ROBOTIS_FRAMEWORK_DYNAMIXEL_SDK_INCLUDE_DYNAMIXEL_SDK_ROBOTISDEF_H_
+#define ROBOTIS_FRAMEWORK_DYNAMIXEL_SDK_INCLUDE_DYNAMIXEL_SDK_ROBOTISDEF_H_
+
+
+typedef char                INT8_T;
+typedef short int           INT16_T;
+typedef int                 INT32_T;
+
+typedef unsigned char       UINT8_T;
+typedef unsigned short int  UINT16_T;
+typedef unsigned int        UINT32_T;
+
+
+#endif /* ROBOTIS_FRAMEWORK_DYNAMIXEL_SDK_INCLUDE_DYNAMIXEL_SDK_ROBOTISDEF_H_ */
--- a/dynamixel_sdk/include/dynamixel_sdk_linux/PortHandlerLinux.h
+++ b/dynamixel_sdk/include/dynamixel_sdk_linux/PortHandlerLinux.h
@@ -0,0 +1,62 @@
+/*
+ * PortHandlerLinux.h
+ *
+ *  Created on: 2016. 1. 26.
+ *      Author: zerom
+ */
+
+#ifndef ROBOTIS_FRAMEWORK_DYNAMIXEL_SDK_INCLUDE_DYNAMIXEL_SDK_LINUX_PORTHANDLERLINUX_H_
+#define ROBOTIS_FRAMEWORK_DYNAMIXEL_SDK_INCLUDE_DYNAMIXEL_SDK_LINUX_PORTHANDLERLINUX_H_
+
+
+#include "dynamixel_sdk/PortHandler.h"
+
+namespace ROBOTIS
+{
+
+class PortHandlerLinux : public PortHandler
+{
+private:
+    int     socket_fd_;
+    int     baudrate_;
+    char    port_name_[30];
+
+    double  packet_start_time_;
+    double  packet_timeout_;
+    double  tx_time_per_byte;
+
+    bool    SetupPort(const int cflag_baud);
+    bool    SetCustomBaudrate(int speed);
+    int     GetCFlagBaud(const int baudrate);
+
+    double  GetCurrentTime();
+    double  GetTimeSinceStart();
+
+public:
+    PortHandlerLinux(const char *port_name);
+    virtual ~PortHandlerLinux() { ClosePort(); }
+
+    bool    OpenPort();
+    void    ClosePort();
+    void    ClearPort();
+
+    void    SetPortName(const char *port_name);
+    char   *GetPortName();
+
+    bool    SetBaudRate(const int baudrate);
+    int     GetBaudRate();
+
+    int     GetBytesAvailable();
+
+    int     ReadPort(UINT8_T *packet, int length);
+    int     WritePort(UINT8_T *packet, int length);
+
+    void    SetPacketTimeout(UINT16_T packet_length);
+    void    SetPacketTimeout(double msec);
+    bool    IsPacketTimeout();
+};
+
+}
+
+
+#endif /* ROBOTIS_FRAMEWORK_DYNAMIXEL_SDK_INCLUDE_DYNAMIXEL_SDK_LINUX_PORTHANDLERLINUX_H_ */
--- a/dynamixel_sdk/package.xml
+++ b/dynamixel_sdk/package.xml
@@ -0,0 +1,15 @@
+<?xml version="1.0"?>
+<package>
+  <name>dynamixel_sdk</name>
+  <version>0.1.0</version>
+  <description>The dynamixel_sdk package</description>
+
+  <maintainer email="zerom@robotis.com">robotis</maintainer>
+  <license>GPLv2</license>
+  <!-- <url type="website">http://wiki.ros.org/dynamixel_sdk</url> -->
+  <author email="zerom@robotis.com">ROBOTIS</author>
+
+  <buildtool_depend>catkin</buildtool_depend>
+  <build_depend>roscpp</build_depend>
+  <run_depend>roscpp</run_depend>
+</package>
--- a/dynamixel_sdk/src/dynamixel_sdk/GroupBulkRead.cpp
+++ b/dynamixel_sdk/src/dynamixel_sdk/GroupBulkRead.cpp
@@ -0,0 +1,202 @@
+/*
+ * GroupBulkRead.cpp
+ *
+ *  Created on: 2016. 1. 28.
+ *      Author: zerom
+ */
+
+#include <stdio.h>
+#include <algorithm>
+#include "dynamixel_sdk/GroupBulkRead.h"
+
+using namespace ROBOTIS;
+
+GroupBulkRead::GroupBulkRead(PortHandler *port, PacketHandler *ph)
+    : port_(port),
+      ph_(ph),
+      param_(0)
+{
+    ClearParam();
+}
+
+void GroupBulkRead::MakeParam()
+{
+    if(id_list_.size() == 0)
+        return;
+
+    if(param_ != 0)
+        delete[] param_;
+    param_ = 0;
+
+    if(ph_->GetProtocolVersion() == 1.0)
+        param_ = new UINT8_T[id_list_.size() * 3];  // ID(1) + ADDR(1) + LENGTH(1)
+    else    // 2.0
+        param_ = new UINT8_T[id_list_.size() * 5];  // ID(1) + ADDR(2) + LENGTH(2)
+
+    int _idx = 0;
+    for(int _i = 0; _i < id_list_.size(); _i++)
+    {
+        UINT8_T _id = id_list_[_i];
+        if(ph_->GetProtocolVersion() == 1.0)
+        {
+            param_[_idx++] = (UINT8_T)length_list_[_id];    // LEN
+            param_[_idx++] = _id;                           // ID
+            param_[_idx++] = (UINT8_T)address_list_[_id];   // ADDR
+        }
+        else    // 2.0
+        {
+            param_[_idx++] = _id;                               // ID
+            param_[_idx++] = DXL_LOBYTE(address_list_[_id]);    // ADDR_L
+            param_[_idx++] = DXL_HIBYTE(address_list_[_id]);    // ADDR_H
+            param_[_idx++] = DXL_LOBYTE(length_list_[_id]);     // LEN_L
+            param_[_idx++] = DXL_HIBYTE(length_list_[_id]);     // LEN_H
+        }
+    }
+}
+
+bool GroupBulkRead::AddParam(UINT8_T id, UINT16_T start_address, UINT16_T data_length)
+{
+    if(std::find(id_list_.begin(), id_list_.end(), id) != id_list_.end())   // id already exist
+        return false;
+
+    id_list_.push_back(id);
+    length_list_[id]    = data_length;
+    address_list_[id]   = start_address;
+    data_list_[id]      = new UINT8_T[data_length];
+
+    MakeParam();
+    return true;
+}
+
+void GroupBulkRead::RemoveParam(UINT8_T id)
+{
+    std::vector<UINT8_T>::iterator it = std::find(id_list_.begin(), id_list_.end(), id);
+    if(it == id_list_.end())    // NOT exist
+        return;
+
+    id_list_.erase(it);
+    address_list_.erase(id);
+    length_list_.erase(id);
+    delete[] data_list_[id];
+    data_list_.erase(id);
+
+    MakeParam();
+}
+
+void GroupBulkRead::ClearParam()
+{
+    if(id_list_.size() != 0)
+    {
+        for(int _i = 0; _i < id_list_.size(); _i++)
+            delete[] data_list_[id_list_[_i]];
+    }
+
+    id_list_.clear();
+    address_list_.clear();
+    length_list_.clear();
+    data_list_.clear();
+    if(param_ != 0)
+        delete[] param_;
+    param_ = 0;
+}
+
+int GroupBulkRead::TxPacket()
+{
+    if(id_list_.size() == 0)
+        return COMM_NOT_AVAILABLE;
+
+    if(ph_->GetProtocolVersion() == 1.0)
+        return ph_->BulkReadTx(port_, param_, id_list_.size() * 3);
+    else    // 2.0
+        return ph_->BulkReadTx(port_, param_, id_list_.size() * 5);
+}
+
+int GroupBulkRead::RxPacket()
+{
+    int _cnt            = id_list_.size();
+    int _result         = COMM_RX_FAIL;
+
+    if(_cnt == 0)
+        return COMM_NOT_AVAILABLE;
+
+    for(int _i = 0; _i < _cnt; _i++)
+    {
+        UINT8_T _id = id_list_[_i];
+
+        _result = ph_->ReadRx(port_, length_list_[_id], data_list_[_id]);
+        if(_result != COMM_SUCCESS)
+        {
+            fprintf(stderr, "[GroupBulkRead::RxPacket] ID %d result : %d !!!!!!!!!!\n", _id, _result);
+            return _result;
+        }
+    }
+
+    return _result;
+}
+
+int GroupBulkRead::TxRxPacket()
+{
+    int _result         = COMM_TX_FAIL;
+
+    _result = TxPacket();
+    if(_result != COMM_SUCCESS)
+        return _result;
+
+    return RxPacket();
+}
+
+bool GroupBulkRead::GetData(UINT8_T id, UINT16_T address, UINT8_T *data)
+{
+    UINT16_T _start_addr, _data_length;
+
+    if(data_list_.find(id) == data_list_.end())
+        return false;
+
+    _start_addr = address_list_[id];
+    _data_length = length_list_[id];
+
+    if(address < _start_addr || _start_addr + _data_length - 1 < address)
+        return false;
+
+    *data = data_list_[id][address - _start_addr];
+
+    return true;
+}
+
+bool GroupBulkRead::GetData(UINT8_T id, UINT16_T address, UINT16_T *data)
+{
+    UINT16_T _start_addr, _data_length;
+
+    if(data_list_.find(id) == data_list_.end())
+        return false;
+
+    _start_addr = address_list_[id];
+    _data_length = length_list_[id];
+
+    if(address < _start_addr || _start_addr + _data_length - 2 < address)
+        return false;
+
+    *data = DXL_MAKEWORD(data_list_[id][address - _start_addr], data_list_[id][address - _start_addr + 1]);
+
+    return true;
+}
+
+bool GroupBulkRead::GetData(UINT8_T id, UINT16_T address, UINT32_T *data)
+{
+    UINT16_T _start_addr, _data_length;
+
+    if(data_list_.find(id) == data_list_.end())
+        return false;
+
+    _start_addr = address_list_[id];
+    _data_length = length_list_[id];
+
+    if(address < _start_addr || _start_addr + _data_length - 4 < address)
+        return false;
+
+    *data = DXL_MAKEDWORD(DXL_MAKEWORD(data_list_[id][address - _start_addr + 0], data_list_[id][address - _start_addr + 1]),
+                          DXL_MAKEWORD(data_list_[id][address - _start_addr + 2], data_list_[id][address - _start_addr + 3]));
+
+    return true;
+}
+
--- a/dynamixel_sdk/src/dynamixel_sdk/GroupBulkWrite.cpp
+++ b/dynamixel_sdk/src/dynamixel_sdk/GroupBulkWrite.cpp
@@ -0,0 +1,133 @@
+/*
+ * GroupBulkWrite.cpp
+ *
+ *  Created on: 2016. 2. 2.
+ *      Author: zerom
+ */
+
+#include <algorithm>
+#include "dynamixel_sdk/GroupBulkWrite.h"
+
+using namespace ROBOTIS;
+
+GroupBulkWrite::GroupBulkWrite(PortHandler *port, PacketHandler *ph)
+    : port_(port),
+      ph_(ph),
+      param_(0),
+      param_length_(0)
+{
+    ClearParam();
+}
+
+void GroupBulkWrite::MakeParam()
+{
+    if(ph_->GetProtocolVersion() == 1.0 || id_list_.size() == 0)
+        return;
+
+    if(param_ != 0)
+        delete[] param_;
+    param_ = 0;
+
+    param_length_ = 0;
+    for(int _i = 0; _i < id_list_.size(); _i++)
+        param_length_ += 1 + 2 + 2 + length_list_[id_list_[_i]];
+
+    param_ = new UINT8_T[param_length_];
+
+    int _idx = 0;
+    for(int _i = 0; _i < id_list_.size(); _i++)
+    {
+        UINT8_T _id = id_list_[_i];
+        if(data_list_[_id] == 0)
+            return;
+
+        param_[_idx++] = _id;
+        param_[_idx++] = DXL_LOBYTE(address_list_[_id]);
+        param_[_idx++] = DXL_HIBYTE(address_list_[_id]);
+        param_[_idx++] = DXL_LOBYTE(length_list_[_id]);
+        param_[_idx++] = DXL_HIBYTE(length_list_[_id]);
+        for(int _c = 0; _c < length_list_[_id]; _c++)
+            param_[_idx++] = (data_list_[_id])[_c];
+    }
+}
+
+bool GroupBulkWrite::AddParam(UINT8_T id, UINT16_T start_address, UINT16_T data_length, UINT8_T *data)
+{
+    if(ph_->GetProtocolVersion() == 1.0)
+        return false;
+
+    if(std::find(id_list_.begin(), id_list_.end(), id) != id_list_.end())   // id already exist
+        return false;
+
+    id_list_.push_back(id);
+    address_list_[id]   = start_address;
+    length_list_[id]    = data_length;
+    data_list_[id] = new UINT8_T[data_length];
+    for(int _c = 0; _c < data_length; _c++)
+        data_list_[id][_c] = data[_c];
+
+    MakeParam();
+    return true;
+}
+void GroupBulkWrite::RemoveParam(UINT8_T id)
+{
+    if(ph_->GetProtocolVersion() == 1.0)
+        return;
+
+    std::vector<UINT8_T>::iterator it = std::find(id_list_.begin(), id_list_.end(), id);
+    if(it == id_list_.end())    // NOT exist
+        return;
+
+    id_list_.erase(it);
+    address_list_.erase(id);
+    length_list_.erase(id);
+    delete[] data_list_[id];
+    data_list_.erase(id);
+
+    MakeParam();
+}
+bool GroupBulkWrite::ChangeParam(UINT8_T id, UINT16_T start_address, UINT16_T data_length, UINT8_T *data)
+{
+    if(ph_->GetProtocolVersion() == 1.0)
+            return false;
+
+    std::vector<UINT8_T>::iterator it = std::find(id_list_.begin(), id_list_.end(), id);
+    if(it == id_list_.end())    // NOT exist
+        return false;
+
+    address_list_[id] = start_address;
+    length_list_[id] = data_length;
+    delete[] data_list_[id];
+    data_list_[id] = new UINT8_T[data_length];
+    for(int _c = 0; _c < data_length; _c++)
+        data_list_[id][_c] = data[_c];
+
+    MakeParam();
+    return true;
+}
+void GroupBulkWrite::ClearParam()
+{
+    if(ph_->GetProtocolVersion() == 1.0)
+        return;
+
+    if(id_list_.size() != 0)
+    {
+        for(int _i = 0; _i < id_list_.size(); _i++)
+            delete[] data_list_[id_list_[_i]];
+    }
+
+    id_list_.clear();
+    address_list_.clear();
+    length_list_.clear();
+    data_list_.clear();
+    if(param_ != 0)
+        delete[] param_;
+    param_ = 0;
+}
+int GroupBulkWrite::TxPacket()
+{
+    if(ph_->GetProtocolVersion() == 1.0 || id_list_.size() == 0)
+        return COMM_NOT_AVAILABLE;
+
+    return ph_->BulkWriteTxOnly(port_, param_, param_length_);
+}
--- a/dynamixel_sdk/src/dynamixel_sdk/GroupSyncRead.cpp
+++ b/dynamixel_sdk/src/dynamixel_sdk/GroupSyncRead.cpp
@@ -0,0 +1,177 @@
+/*
+ * GroupSyncRead.cpp
+ *
+ *  Created on: 2016. 2. 2.
+ *      Author: zerom
+ */
+
+#include <algorithm>
+#include "dynamixel_sdk/GroupSyncRead.h"
+
+using namespace ROBOTIS;
+
+GroupSyncRead::GroupSyncRead(PortHandler *port, PacketHandler *ph, UINT16_T start_address, UINT16_T data_length)
+    : port_(port),
+      ph_(ph),
+      param_(0),
+      start_address_(start_address),
+      data_length_(data_length)
+{
+    ClearParam();
+}
+
+void GroupSyncRead::MakeParam()
+{
+    if(ph_->GetProtocolVersion() == 1.0 || id_list_.size() == 0)
+        return;
+
+    if(param_ != 0)
+        delete[] param_;
+    param_ = 0;
+
+    param_ = new UINT8_T[id_list_.size() * 1];  // ID(1)
+
+    int _idx = 0;
+    for(int _i = 0; _i < id_list_.size(); _i++)
+        param_[_idx++] = id_list_[_i];
+}
+
+bool GroupSyncRead::AddParam(UINT8_T id)
+{
+    if(ph_->GetProtocolVersion() == 1.0)
+        return false;
+
+    if(std::find(id_list_.begin(), id_list_.end(), id) != id_list_.end())   // id already exist
+        return false;
+
+    id_list_.push_back(id);
+    data_list_[id] = new UINT8_T[data_length_];
+
+    MakeParam();
+    return true;
+}
+void GroupSyncRead::RemoveParam(UINT8_T id)
+{
+    if(ph_->GetProtocolVersion() == 1.0)
+        return;
+
+    std::vector<UINT8_T>::iterator it = std::find(id_list_.begin(), id_list_.end(), id);
+    if(it == id_list_.end())    // NOT exist
+        return;
+
+    id_list_.erase(it);
+    delete[] data_list_[id];
+    data_list_.erase(id);
+
+    MakeParam();
+}
+void GroupSyncRead::ClearParam()
+{
+    if(ph_->GetProtocolVersion() == 1.0)
+        return;
+
+    if(id_list_.size() != 0)
+    {
+        for(int _i = 0; _i < id_list_.size(); _i++)
+            delete[] data_list_[id_list_[_i]];
+    }
+
+    id_list_.clear();
+    data_list_.clear();
+    if(param_ != 0)
+        delete[] param_;
+    param_ = 0;
+}
+
+int GroupSyncRead::TxPacket()
+{
+    if(ph_->GetProtocolVersion() == 1.0 || id_list_.size() == 0)
+        return COMM_NOT_AVAILABLE;
+
+    return ph_->SyncReadTx(port_, start_address_, data_length_, param_, (UINT16_T)id_list_.size() * 1);
+}
+
+int GroupSyncRead::RxPacket()
+{
+    if(ph_->GetProtocolVersion() == 1.0)
+        return COMM_NOT_AVAILABLE;
+
+    int _cnt            = id_list_.size();
+    int _result         = COMM_RX_FAIL;
+
+    if(_cnt == 0)
+        return COMM_NOT_AVAILABLE;
+
+    for(int _i = 0; _i < _cnt; _i++)
+    {
+        UINT8_T _id = id_list_[_i];
+
+        _result = ph_->ReadRx(port_, data_length_, data_list_[_id]);
+        if(_result != COMM_SUCCESS)
+            return _result;
+    }
+
+    return _result;
+}
+
+int GroupSyncRead::TxRxPacket()
+{
+    if(ph_->GetProtocolVersion() == 1.0)
+        return COMM_NOT_AVAILABLE;
+
+    int _result         = COMM_TX_FAIL;
+
+    _result = TxPacket();
+    if(_result != COMM_SUCCESS)
+        return _result;
+
+    return RxPacket();
+}
+
+bool GroupSyncRead::GetData(UINT8_T id, UINT16_T address, UINT8_T *data)
+{
+    if(ph_->GetProtocolVersion() == 1.0)
+        return false;
+
+    if(data_list_.find(id) == data_list_.end())
+        return false;
+
+    if(address < start_address_ || start_address_ + data_length_ - 1 < address)
+        return false;
+
+    *data = data_list_[id][address - start_address_];
+
+    return true;
+}
+bool GroupSyncRead::GetData(UINT8_T id, UINT16_T address, UINT16_T *data)
+{
+    if(ph_->GetProtocolVersion() == 1.0)
+        return false;
+
+    if(data_list_.find(id) == data_list_.end())
+        return false;
+
+    if(address < start_address_ || start_address_ + data_length_ - 2 < address)
+        return false;
+
+    *data = DXL_MAKEWORD(data_list_[id][address - start_address_], data_list_[id][address - start_address_ + 1]);
+
+    return true;
+}
+bool GroupSyncRead::GetData(UINT8_T id, UINT16_T address, UINT32_T *data)
+{
+    if(ph_->GetProtocolVersion() == 1.0)
+        return false;
+
+    if(data_list_.find(id) == data_list_.end())
+        return false;
+
+    if(address < start_address_ || start_address_ + data_length_ - 4 < address)
+        return false;
+
+    *data = DXL_MAKEDWORD(DXL_MAKEWORD(data_list_[id][address - start_address_ + 0], data_list_[id][address - start_address_ + 1]),
+                          DXL_MAKEWORD(data_list_[id][address - start_address_ + 2], data_list_[id][address - start_address_ + 3]));
+
+    return true;
+}
+
--- a/dynamixel_sdk/src/dynamixel_sdk/GroupSyncWrite.cpp
+++ b/dynamixel_sdk/src/dynamixel_sdk/GroupSyncWrite.cpp
@@ -0,0 +1,110 @@
+/*
+ * GroupSyncWrite.cpp
+ *
+ *  Created on: 2016. 1. 28.
+ *      Author: zerom
+ */
+
+#include <algorithm>
+#include "dynamixel_sdk/GroupSyncWrite.h"
+
+using namespace ROBOTIS;
+
+GroupSyncWrite::GroupSyncWrite(PortHandler *port, PacketHandler *ph, UINT16_T start_address, UINT16_T data_length)
+    : port_(port),
+      ph_(ph),
+      param_(0),
+      start_address_(start_address),
+      data_length_(data_length)
+{
+    ClearParam();
+}
+
+void GroupSyncWrite::MakeParam()
+{
+    if(id_list_.size() == 0)
+        return;
+
+    if(param_ != 0)
+        delete[] param_;
+    param_ = 0;
+
+    param_ = new UINT8_T[id_list_.size() * (1 + data_length_)]; // ID(1) + DATA(data_length)
+
+    int _idx = 0;
+    for(int _i = 0; _i < id_list_.size(); _i++)
+    {
+        UINT8_T _id = id_list_[_i];
+        if(data_list_[_id] == 0)
+            return;
+
+        param_[_idx++] = _id;
+        for(int _c = 0; _c < data_length_; _c++)
+            param_[_idx++] = (data_list_[_id])[_c];
+    }
+}
+
+bool GroupSyncWrite::AddParam(UINT8_T id, UINT8_T *data)
+{
+    if(std::find(id_list_.begin(), id_list_.end(), id) != id_list_.end())   // id already exist
+        return false;
+
+    id_list_.push_back(id);
+    data_list_[id] = new UINT8_T[data_length_];
+    for(int _c = 0; _c < data_length_; _c++)
+        data_list_[id][_c] = data[_c];
+
+    MakeParam();
+    return true;
+}
+
+void GroupSyncWrite::RemoveParam(UINT8_T id)
+{
+    std::vector<UINT8_T>::iterator it = std::find(id_list_.begin(), id_list_.end(), id);
+    if(it == id_list_.end())    // NOT exist
+        return;
+
+    id_list_.erase(it);
+    delete[] data_list_[id];
+    data_list_.erase(id);
+
+    MakeParam();
+}
+
+bool GroupSyncWrite::ChangeParam(UINT8_T id, UINT8_T *data)
+{
+    std::vector<UINT8_T>::iterator it = std::find(id_list_.begin(), id_list_.end(), id);
+    if(it == id_list_.end())    // NOT exist
+        return false;
+
+    delete[] data_list_[id];
+    data_list_[id] = new UINT8_T[data_length_];
+    for(int _c = 0; _c < data_length_; _c++)
+        data_list_[id][_c] = data[_c];
+
+    MakeParam();
+    return true;
+}
+
+void GroupSyncWrite::ClearParam()
+{
+    if(id_list_.size() != 0)
+    {
+        for(int _i = 0; _i < id_list_.size(); _i++)
+            delete[] data_list_[id_list_[_i]];
+    }
+
+    id_list_.clear();
+    data_list_.clear();
+    if(param_ != 0)
+        delete[] param_;
+    param_ = 0;
+}
+
+int GroupSyncWrite::TxPacket()
+{
+    if(id_list_.size() == 0)
+        return COMM_NOT_AVAILABLE;
+
+    return ph_->SyncWriteTxOnly(port_, start_address_, data_length_, param_, id_list_.size() * (1 + data_length_));
+}
--- a/dynamixel_sdk/src/dynamixel_sdk/PacketHandler.cpp
+++ b/dynamixel_sdk/src/dynamixel_sdk/PacketHandler.cpp
@@ -0,0 +1,22 @@
+/*
+ * PacketHandler.cpp
+ *
+ *  Created on: 2016. 1. 26.
+ *      Author: zerom
+ */
+
+#include "dynamixel_sdk/PacketHandler.h"
+#include "dynamixel_sdk/Protocol1PacketHandler.h"
+#include "dynamixel_sdk/Protocol2PacketHandler.h"
+
+using namespace ROBOTIS;
+
+PacketHandler *PacketHandler::GetPacketHandler(float protocol_version)
+{
+    if(protocol_version == 1.0)
+        return (PacketHandler *)(Protocol1PacketHandler::GetInstance());
+    else if(protocol_version == 2.0)
+        return (PacketHandler *)(Protocol2PacketHandler::GetInstance());
+
+    return (PacketHandler *)(Protocol2PacketHandler::GetInstance());
+}
--- a/dynamixel_sdk/src/dynamixel_sdk/PortHandler.cpp
+++ b/dynamixel_sdk/src/dynamixel_sdk/PortHandler.cpp
@@ -0,0 +1,31 @@
+/*
+ * PortHandler.cpp
+ *
+ *  Created on: 2016. 2. 5.
+ *      Author: zerom
+ */
+
+#include "dynamixel_sdk/PortHandler.h"
+
+#ifdef __linux__
+  #include "dynamixel_sdk_linux/PortHandlerLinux.h"
+#endif
+
+#ifdef _WIN32 || _WIN64
+  #include "dynamixel_sdk_windows/PortHandlerWindows.h"
+#endif
+
+using namespace ROBOTIS;
+
+PortHandler *PortHandler::GetPortHandler(const char *port_name)
+{
+#ifdef __linux__
+    return (PortHandler *)(new PortHandlerLinux(port_name));
+#endif
+
+#ifdef _WIN32 || _WIN64
+    return (PortHandler *)(new PortHandlerWindows(port_name));
+#endif
+}
+
+
--- a/dynamixel_sdk/src/dynamixel_sdk/Protocol1PacketHandler.cpp
+++ b/dynamixel_sdk/src/dynamixel_sdk/Protocol1PacketHandler.cpp
@@ -0,0 +1,562 @@
+/*
+ * Protocol1PacketHandler.cpp
+ *
+ *  Created on: 2016. 1. 26.
+ *      Author: zerom
+ */
+
+#include <string.h>
+#include <stdlib.h>
+#include "dynamixel_sdk/Protocol1PacketHandler.h"
+
+#define TXPACKET_MAX_LEN    (250)
+#define RXPACKET_MAX_LEN    (250)
+
+///////////////// for Protocol 1.0 Packet /////////////////
+#define PKT_HEADER0             0
+#define PKT_HEADER1             1
+#define PKT_ID                  2
+#define PKT_LENGTH              3
+#define PKT_INSTRUCTION         4
+#define PKT_ERROR               4
+#define PKT_PARAMETER0          5
+
+///////////////// Protocol 1.0 Error bit /////////////////
+#define ERRBIT_VOLTAGE          1       // Supplied voltage is out of the range (operating volatage set in the control table)
+#define ERRBIT_ANGLE            2       // Goal position is written out of the range (from CW angle limit to CCW angle limit)
+#define ERRBIT_OVERHEAT         4       // Temperature is out of the range (operating temperature set in the control table)
+#define ERRBIT_RANGE            8       // Command(setting value) is out of the range for use.
+#define ERRBIT_CHECKSUM         16      // Instruction packet checksum is incorrect.
+#define ERRBIT_OVERLOAD         32      // The current load cannot be controlled by the set torque.
+#define ERRBIT_INSTRUCTION      64      // Undefined instruction or delivering the action command without the reg_write command.
+
+using namespace ROBOTIS;
+
+Protocol1PacketHandler *Protocol1PacketHandler::unique_instance_ = new Protocol1PacketHandler();
+
+Protocol1PacketHandler::Protocol1PacketHandler() { }
+
+int Protocol1PacketHandler::TxPacket(PortHandler *port, UINT8_T *txpacket)
+{
+    UINT8_T _checksum               = 0;
+    UINT8_T _total_packet_length    = txpacket[PKT_LENGTH] + 4; // 4: HEADER0 HEADER1 ID LENGTH
+    UINT8_T _written_packet_length  = 0;
+
+    if(port->is_using)
+        return COMM_PORT_BUSY;
+    port->is_using = true;
+
+    // check max packet length
+    if(_total_packet_length > TXPACKET_MAX_LEN)
+    {
+        port->is_using = false;
+        return COMM_TX_ERROR;
+    }
+
+    // make packet header
+    txpacket[PKT_HEADER0]   = 0xFF;
+    txpacket[PKT_HEADER1]   = 0xFF;
+
+    // add a checksum to the packet
+    for(int _idx = 2; _idx < _total_packet_length - 1; _idx++)   // except header, checksum
+        _checksum += txpacket[_idx];
+    txpacket[_total_packet_length - 1] = ~_checksum;
+
+    // tx packet
+    port->ClearPort();
+    _written_packet_length = port->WritePort(txpacket, _total_packet_length);
+    if(_total_packet_length != _written_packet_length)
+    {
+        port->is_using = false;
+        return COMM_TX_FAIL;
+    }
+
+    return COMM_SUCCESS;
+}
+
+int Protocol1PacketHandler::RxPacket(PortHandler *port, UINT8_T *rxpacket)
+{
+    int     _result         = COMM_TX_FAIL;
+
+    UINT8_T _checksum       = 0;
+    UINT8_T _rx_length      = 0;
+    UINT8_T _wait_length    = 6;    // minimum length ( HEADER0 HEADER1 ID LENGTH ERROR CHKSUM )
+
+    while(true)
+    {
+        _rx_length += port->ReadPort(&rxpacket[_rx_length], _wait_length - _rx_length);
+        if(_rx_length >= _wait_length)
+        {
+            UINT8_T _idx = 0;
+
+            // find packet header
+            for(_idx = 0; _idx < (_rx_length - 1); _idx++)
+            {
+                if(rxpacket[_idx] == 0xFF && rxpacket[_idx+1] == 0xFF)
+                    break;
+            }
+
+            if(_idx == 0)   // found at the beginning of the packet
+            {
+                // re-calculate the exact length of the rx packet
+                _wait_length = rxpacket[PKT_LENGTH] + PKT_LENGTH + 1;
+                if(_rx_length < _wait_length)
+                {
+                    // check timeout
+                    if(port->IsPacketTimeout() == true)
+                    {
+                        if(_rx_length == 0)
+                            _result = COMM_RX_TIMEOUT;
+                        else
+                            _result = COMM_RX_CORRUPT;
+                        break;
+                    }
+                    else
+                        continue;
+                }
+
+                // calculate checksum
+                for(int _i = 2; _i < _wait_length - 1; _i++)   // except header, checksum
+                    _checksum += rxpacket[_i];
+                _checksum = ~_checksum;
+
+                // verify checksum
+                if(rxpacket[_wait_length - 1] == _checksum)
+                    _result = COMM_SUCCESS;
+                else
+                    _result = COMM_RX_CORRUPT;
+                break;
+            }
+            else
+            {
+                // remove unnecessary packets
+                for(UINT8_T _s = 0; _s < _rx_length - _idx; _s++)
+                    rxpacket[_s] = rxpacket[_idx + _s];
+                //memcpy(&rxpacket[0], &rxpacket[_idx], _rx_length - _idx);
+                _rx_length -= _idx;
+            }
+        }
+        else
+        {
+            // check timeout
+            if(port->IsPacketTimeout() == true)
+            {
+                if(_rx_length == 0)
+                    _result = COMM_RX_TIMEOUT;
+                else
+                    _result = COMM_RX_CORRUPT;
+                break;
+            }
+        }
+    }
+    port->is_using = false;
+
+    return _result;
+}
+
+// NOT for BulkRead instruction
+int Protocol1PacketHandler::TxRxPacket(PortHandler *port, UINT8_T *txpacket, UINT8_T *rxpacket, UINT8_T *error)
+{
+    int _result = COMM_TX_FAIL;
+
+    // tx packet
+    _result = TxPacket(port, txpacket);
+    if(_result != COMM_SUCCESS)
+        return _result;
+
+    // (ID == Broadcast ID && NOT BulkRead) == no need to wait for status packet
+    // (Instruction == Action) == no need to wait for status packet
+    if((txpacket[PKT_ID] == BROADCAST_ID && txpacket[PKT_INSTRUCTION] != INST_BULK_READ) ||
+       (txpacket[PKT_INSTRUCTION] == INST_ACTION))
+    {
+        port->is_using = false;
+        return _result;
+    }
+
+    // set packet timeout
+    if(txpacket[PKT_INSTRUCTION] == INST_READ)
+        port->SetPacketTimeout((UINT16_T)(txpacket[PKT_PARAMETER0+1] + 6));
+    else
+        port->SetPacketTimeout((UINT16_T)6);
+
+    // rx packet
+    _result = RxPacket(port, rxpacket);
+    // TODO: check txpacket ID == rxpacket ID
+
+    if(_result == COMM_SUCCESS && txpacket[PKT_ID] != BROADCAST_ID)
+    {
+        if(error != 0)
+            *error = (UINT8_T)rxpacket[PKT_ERROR];
+    }
+
+    return _result;
+}
+
+int Protocol1PacketHandler::Ping(PortHandler *port, UINT8_T id, UINT8_T *error)
+{
+    return Ping(port, id, 0, error);
+}
+
+int Protocol1PacketHandler::Ping(PortHandler *port, UINT8_T id, UINT16_T *model_number, UINT8_T *error)
+{
+    int _result                 = COMM_TX_FAIL;
+
+    UINT8_T txpacket[6]         = {0};
+    UINT8_T rxpacket[6]         = {0};
+
+    if(id >= BROADCAST_ID)
+        return COMM_NOT_AVAILABLE;
+
+    txpacket[PKT_ID]            = id;
+    txpacket[PKT_LENGTH]        = 2;
+    txpacket[PKT_INSTRUCTION]   = INST_PING;
+
+    _result = TxRxPacket(port, txpacket, rxpacket, error);
+    if(_result == COMM_SUCCESS && model_number != 0)
+    {
+        UINT8_T _data[2] = {0};
+        _result = ReadTxRx(port, id, 0, 2, _data);  // Address 0 : Model Number
+        if(_result == COMM_SUCCESS)
+            *model_number = DXL_MAKEWORD(_data[0], _data[1]);
+    }
+
+    return _result;
+}
+
+int Protocol1PacketHandler::BroadcastPing(PortHandler *port, std::vector<UINT8_T> &id_list)
+{
+    return COMM_NOT_AVAILABLE;
+}
+
+int Protocol1PacketHandler::Action(PortHandler *port, UINT8_T id)
+{
+    UINT8_T txpacket[6]         = {0};
+
+    txpacket[PKT_ID]            = id;
+    txpacket[PKT_LENGTH]        = 2;
+    txpacket[PKT_INSTRUCTION]   = INST_ACTION;
+
+    return TxRxPacket(port, txpacket, 0);
+}
+
+int Protocol1PacketHandler::Reboot(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};
+    UINT8_T rxpacket[6]         = {0};
+
+    txpacket[PKT_ID]            = id;
+    txpacket[PKT_LENGTH]        = 2;
+    txpacket[PKT_INSTRUCTION]   = INST_FACTORY_RESET;
+
+    return TxRxPacket(port, txpacket, rxpacket, error);
+}
+
+int Protocol1PacketHandler::ReadTx(PortHandler *port, UINT8_T id, UINT16_T address, UINT16_T length)
+{
+    int _result                 = COMM_TX_FAIL;
+
+    UINT8_T txpacket[8]         = {0};
+
+    if(id >= BROADCAST_ID)
+        return COMM_NOT_AVAILABLE;
+
+    txpacket[PKT_ID]            = id;
+    txpacket[PKT_LENGTH]        = 4;
+    txpacket[PKT_INSTRUCTION]   = INST_READ;
+    txpacket[PKT_PARAMETER0+0]  = (UINT8_T)address;
+    txpacket[PKT_PARAMETER0+1]  = (UINT8_T)length;
+
+    _result = TxPacket(port, txpacket);
+
+    // set packet timeout
+    if(_result == COMM_SUCCESS)
+        port->SetPacketTimeout((UINT16_T)(length+6));
+
+    return _result;
+}
+
+int Protocol1PacketHandler::ReadRx(PortHandler *port, UINT16_T length, UINT8_T *data, UINT8_T *error)
+{
+    int _result                 = COMM_TX_FAIL;
+    UINT8_T *rxpacket           = (UINT8_T *)malloc(RXPACKET_MAX_LEN);//(length+6);
+    //UINT8_T *rxpacket           = new UINT8_T[length+6];
+
+    _result = RxPacket(port, rxpacket);
+    if(_result == COMM_SUCCESS)
+    {
+        if(error != 0)
+            *error = (UINT8_T)rxpacket[PKT_ERROR];
+        for(UINT8_T _s = 0; _s < length; _s++)
+            data[_s] = rxpacket[PKT_PARAMETER0 + _s];
+        //memcpy(data, &rxpacket[PKT_PARAMETER0], length);
+    }
+
+    free(rxpacket);
+    //delete[] rxpacket;
+    return _result;
+}
+
+int Protocol1PacketHandler::ReadTxRx(PortHandler *port, UINT8_T id, UINT16_T address, UINT16_T length, UINT8_T *data, UINT8_T *error)
+{
+    int _result = COMM_TX_FAIL;
+
+    _result = ReadTx(port, id, address, length);
+    if(_result != COMM_SUCCESS)
+        return _result;
+
+    return ReadRx(port, length, data, error);
+}
+
+int Protocol1PacketHandler::Read1ByteTx(PortHandler *port, UINT8_T id, UINT16_T address)
+{
+    return ReadTx(port, id, address, 1);
+}
+int Protocol1PacketHandler::Read1ByteRx(PortHandler *port, UINT8_T *data, UINT8_T *error)
+{
+    UINT8_T _data[1] = {0};
+    int _result = ReadRx(port, 1, _data, error);
+    *data = _data[0];
+    return _result;
+}
+int Protocol1PacketHandler::Read1ByteTxRx(PortHandler *port, UINT8_T id, UINT16_T address, UINT8_T *data, UINT8_T *error)
+{
+    int _result = COMM_TX_FAIL;
+
+    _result = Read1ByteTx(port, id, address);
+    if(_result != COMM_SUCCESS)
+        return _result;
+
+    return Read1ByteRx(port, data, error);
+}
+
+int Protocol1PacketHandler::Read2ByteTx(PortHandler *port, UINT8_T id, UINT16_T address)
+{
+    return ReadTx(port, id, address, 2);
+}
+int Protocol1PacketHandler::Read2ByteRx(PortHandler *port, UINT16_T *data, UINT8_T *error)
+{
+    UINT8_T _data[2] = {0};
+    int _result = ReadRx(port, 2, _data, error);
+    *data = DXL_MAKEWORD(_data[0], _data[1]);
+    return _result;
+}
+int Protocol1PacketHandler::Read2ByteTxRx(PortHandler *port, UINT8_T id, UINT16_T address, UINT16_T *data, UINT8_T *error)
+{
+    int _result = COMM_TX_FAIL;
+
+    _result = Read2ByteTx(port, id, address);
+    if(_result != COMM_SUCCESS)
+        return _result;
+
+    return Read2ByteRx(port, data, error);
+}
+
+int Protocol1PacketHandler::Read4ByteTx(PortHandler *port, UINT8_T id, UINT16_T address)
+{
+    return COMM_NOT_AVAILABLE;
+}
+int Protocol1PacketHandler::Read4ByteRx(PortHandler *port, UINT32_T *data, UINT8_T *error)
+{
+    return COMM_NOT_AVAILABLE;
+}
+int Protocol1PacketHandler::Read4ByteTxRx(PortHandler *port, UINT8_T id, UINT16_T address, UINT32_T *data, UINT8_T *error)
+{
+    return COMM_NOT_AVAILABLE;
+}
+
+int Protocol1PacketHandler::WriteTxOnly(PortHandler *port, UINT8_T id, UINT16_T address, UINT16_T length, UINT8_T *data)
+{
+    int _result                 = COMM_TX_FAIL;
+
+    UINT8_T *txpacket           = (UINT8_T *)malloc(length+7);
+    //UINT8_T *txpacket           = new UINT8_T[length+7];
+
+    txpacket[PKT_ID]            = id;
+    txpacket[PKT_LENGTH]        = length+3;
+    txpacket[PKT_INSTRUCTION]   = INST_WRITE;
+    txpacket[PKT_PARAMETER0]    = (UINT8_T)address;
+
+    for(UINT8_T _s = 0; _s < length; _s++)
+        txpacket[PKT_PARAMETER0+1+_s] = data[_s];
+    //memcpy(&txpacket[PKT_PARAMETER0+1], data, length);
+
+    _result = TxPacket(port, txpacket);
+    port->is_using = false;
+
+    free(txpacket);
+    //delete[] txpacket;
+    return _result;
+}
+
+int Protocol1PacketHandler::WriteTxRx(PortHandler *port, UINT8_T id, UINT16_T address, UINT16_T length, UINT8_T *data, UINT8_T *error)
+{
+    int _result                 = COMM_TX_FAIL;
+
+    UINT8_T *txpacket           = (UINT8_T *)malloc(length+6);
+    //UINT8_T *txpacket           = new UINT8_T[length+6];
+    UINT8_T rxpacket[6]         = {0};
+
+    txpacket[PKT_ID]            = id;
+    txpacket[PKT_LENGTH]        = length+3;
+    txpacket[PKT_INSTRUCTION]   = INST_WRITE;
+    txpacket[PKT_PARAMETER0]    = (UINT8_T)address;
+
+    for(UINT8_T _s = 0; _s < length; _s++)
+        txpacket[PKT_PARAMETER0+1+_s] = data[_s];
+    //memcpy(&txpacket[PKT_PARAMETER0+1], data, length);
+
+    _result = TxRxPacket(port, txpacket, rxpacket, error);
+
+    free(txpacket);
+    //delete[] txpacket;
+    return _result;
+}
+
+int Protocol1PacketHandler::Write1ByteTxOnly(PortHandler *port, UINT8_T id, UINT16_T address, UINT8_T data)
+{
+    UINT8_T _data[1] = { data };
+    return WriteTxOnly(port, id, address, 1, _data);
+}
+int Protocol1PacketHandler::Write1ByteTxRx(PortHandler *port, UINT8_T id, UINT16_T address, UINT8_T data, UINT8_T *error)
+{
+    UINT8_T _data[1] = { data };
+    return WriteTxRx(port, id, address, 1, _data, error);
+}
+
+int Protocol1PacketHandler::Write2ByteTxOnly(PortHandler *port, UINT8_T id, UINT16_T address, UINT16_T data)
+{
+    UINT8_T _data[2] = { DXL_LOBYTE(data), DXL_HIBYTE(data) };
+    return WriteTxOnly(port, id, address, 2, _data);
+}
+int Protocol1PacketHandler::Write2ByteTxRx(PortHandler *port, UINT8_T id, UINT16_T address, UINT16_T data, UINT8_T *error)
+{
+    UINT8_T _data[2] = { DXL_LOBYTE(data), DXL_HIBYTE(data) };
+    return WriteTxRx(port, id, address, 2, _data, error);
+}
+
+int Protocol1PacketHandler::Write4ByteTxOnly(PortHandler *port, UINT8_T id, UINT16_T address, UINT32_T data)
+{
+    return COMM_NOT_AVAILABLE;
+}
+int Protocol1PacketHandler::Write4ByteTxRx(PortHandler *port, UINT8_T id, UINT16_T address, UINT32_T data, UINT8_T *error)
+{
+    return COMM_NOT_AVAILABLE;
+}
+
+int Protocol1PacketHandler::RegWriteTxOnly(PortHandler *port, UINT8_T id, UINT16_T address, UINT16_T length, UINT8_T *data)
+{
+    int _result                 = COMM_TX_FAIL;
+
+    UINT8_T *txpacket           = (UINT8_T *)malloc(length+6);
+    //UINT8_T *txpacket           = new UINT8_T[length+6];
+
+    txpacket[PKT_ID]            = id;
+    txpacket[PKT_LENGTH]        = length+3;
+    txpacket[PKT_INSTRUCTION]   = INST_REG_WRITE;
+    txpacket[PKT_PARAMETER0]    = (UINT8_T)address;
+
+    for(UINT8_T _s = 0; _s < length; _s++)
+        txpacket[PKT_PARAMETER0+1+_s] = data[_s];
+    //memcpy(&txpacket[PKT_PARAMETER0+1], data, length);
+
+    _result = TxPacket(port, txpacket);
+    port->is_using = false;
+
+    free(txpacket);
+    //delete[] txpacket;
+    return _result;
+}
+
+int Protocol1PacketHandler::RegWriteTxRx(PortHandler *port, UINT8_T id, UINT16_T address, UINT16_T length, UINT8_T *data, UINT8_T *error)
+{
+    int _result                 = COMM_TX_FAIL;
+
+    UINT8_T *txpacket           = (UINT8_T *)malloc(length+6);
+    //UINT8_T *txpacket           = new UINT8_T[length+6];
+    UINT8_T rxpacket[6]         = {0};
+
+    txpacket[PKT_ID]            = id;
+    txpacket[PKT_LENGTH]        = length+3;
+    txpacket[PKT_INSTRUCTION]   = INST_REG_WRITE;
+    txpacket[PKT_PARAMETER0]    = (UINT8_T)address;
+
+    for(UINT8_T _s = 0; _s < length; _s++)
+        txpacket[PKT_PARAMETER0+1+_s] = data[_s];
+    //memcpy(&txpacket[PKT_PARAMETER0+1], data, length);
+
+    _result = TxRxPacket(port, txpacket, rxpacket, error);
+
+    free(txpacket);
+    //delete[] txpacket;
+    return _result;
+}
+
+int Protocol1PacketHandler::SyncReadTx(PortHandler *port, UINT16_T start_address, UINT16_T data_length, UINT8_T *param, UINT16_T param_length)
+{
+    return COMM_NOT_AVAILABLE;
+}
+
+int Protocol1PacketHandler::SyncWriteTxOnly(PortHandler *port, UINT16_T start_address, UINT16_T data_length, UINT8_T *param, UINT16_T param_length)
+{
+    int _result                 = COMM_TX_FAIL;
+
+    UINT8_T *txpacket           = (UINT8_T *)malloc(param_length+8);    // 8: HEADER0 HEADER1 ID LEN INST START_ADDR DATA_LEN ... CHKSUM
+    //UINT8_T *txpacket           = new UINT8_T[param_length + 8];    // 8: HEADER0 HEADER1 ID LEN INST START_ADDR DATA_LEN ... CHKSUM
+
+    txpacket[PKT_ID]            = BROADCAST_ID;
+    txpacket[PKT_LENGTH]        = param_length + 4; // 4: INST START_ADDR DATA_LEN ... CHKSUM
+    txpacket[PKT_INSTRUCTION]   = INST_SYNC_WRITE;
+    txpacket[PKT_PARAMETER0+0]  = start_address;
+    txpacket[PKT_PARAMETER0+1]  = data_length;
+
+    for(UINT8_T _s = 0; _s < param_length; _s++)
+        txpacket[PKT_PARAMETER0+2+_s] = param[_s];
+    //memcpy(&txpacket[PKT_PARAMETER0+2], param, param_length);
+
+    _result = TxRxPacket(port, txpacket, 0, 0);
+
+    free(txpacket);
+    //delete[] txpacket;
+    return _result;
+}
+
+int Protocol1PacketHandler::BulkReadTx(PortHandler *port, UINT8_T *param, UINT16_T param_length)
+{
+    int _result                 = COMM_TX_FAIL;
+
+    UINT8_T *txpacket           = (UINT8_T *)malloc(param_length+7);    // 7: HEADER0 HEADER1 ID LEN INST 0x00 ... CHKSUM
+    //UINT8_T *txpacket           = new UINT8_T[param_length + 7];    // 7: HEADER0 HEADER1 ID LEN INST 0x00 ... CHKSUM
+
+    txpacket[PKT_ID]            = BROADCAST_ID;
+    txpacket[PKT_LENGTH]        = param_length + 3; // 3: INST 0x00 ... CHKSUM
+    txpacket[PKT_INSTRUCTION]   = INST_BULK_READ;
+    txpacket[PKT_PARAMETER0+0]  = 0x00;
+
+    for(UINT8_T _s = 0; _s < param_length; _s++)
+        txpacket[PKT_PARAMETER0+1+_s] = param[_s];
+    //memcpy(&txpacket[PKT_PARAMETER0+1], param, param_length);
+
+    _result = TxPacket(port, txpacket);
+    if(_result == COMM_SUCCESS)
+    {
+        int _wait_length = 0;
+        for(int _i = 0; _i < param_length; _i += 3)
+            _wait_length += param[_i] + 7;
+        port->SetPacketTimeout((UINT16_T)_wait_length);
+    }
+
+    free(txpacket);
+    //delete[] txpacket;
+    return _result;
+}
+
+int Protocol1PacketHandler::BulkWriteTxOnly(PortHandler *port, UINT8_T *param, UINT16_T param_length)
+{
+    return COMM_NOT_AVAILABLE;
+}
+
--- a/dynamixel_sdk/src/dynamixel_sdk/Protocol2PacketHandler.cpp
+++ b/dynamixel_sdk/src/dynamixel_sdk/Protocol2PacketHandler.cpp
@@ -0,0 +1,898 @@
+/*
+ * Protocol2PacketHandler.cpp
+ *
+ *  Created on: 2016. 1. 26.
+ *      Author: zerom
+ */
+
+#include <stdio.h>
+#include <string.h>
+#include <stdlib.h>
+#include "dynamixel_sdk/Protocol2PacketHandler.h"
+
+#define TXPACKET_MAX_LEN    (4*1024)
+#define RXPACKET_MAX_LEN    (4*1024)
+
+///////////////// for Protocol 2.0 Packet /////////////////
+#define PKT_HEADER0             0
+#define PKT_HEADER1             1
+#define PKT_HEADER2             2
+#define PKT_RESERVED            3
+#define PKT_ID                  4
+#define PKT_LENGTH_L            5
+#define PKT_LENGTH_H            6
+#define PKT_INSTRUCTION         7
+#define PKT_ERROR               8
+#define PKT_PARAMETER0          8
+
+///////////////// Protocol 2.0 Error bit /////////////////
+#define ERRNUM_RESULT_FAIL      1       // Failed to process the instruction packet.
+#define ERRNUM_INSTRUCTION      2       // Instruction error
+#define ERRNUM_CRC              3       // CRC check error
+#define ERRNUM_DATA_RANGE       4       // Data range error
+#define ERRNUM_DATA_LENGTH      5       // Data length error
+#define ERRNUM_DATA_LIMIT       6       // Data limit error
+#define ERRNUM_ACCESS           7       // Access error
+
+#define ERRBIT_ALERT            128     //When the device has a problem, this bit is set to 1. Check "Device Status Check" value.
+
+using namespace ROBOTIS;
+
+Protocol2PacketHandler *Protocol2PacketHandler::unique_instance_ = new Protocol2PacketHandler();
+
+Protocol2PacketHandler::Protocol2PacketHandler() { }
+
+unsigned short Protocol2PacketHandler::UpdateCRC(UINT16_T crc_accum, UINT8_T *data_blk_ptr, UINT16_T data_blk_size)
+{
+    UINT16_T i, j;
+    UINT16_T crc_table[256] = {0x0000,
+    0x8005, 0x800F, 0x000A, 0x801B, 0x001E, 0x0014, 0x8011,
+    0x8033, 0x0036, 0x003C, 0x8039, 0x0028, 0x802D, 0x8027,
+    0x0022, 0x8063, 0x0066, 0x006C, 0x8069, 0x0078, 0x807D,
+    0x8077, 0x0072, 0x0050, 0x8055, 0x805F, 0x005A, 0x804B,
+    0x004E, 0x0044, 0x8041, 0x80C3, 0x00C6, 0x00CC, 0x80C9,
+    0x00D8, 0x80DD, 0x80D7, 0x00D2, 0x00F0, 0x80F5, 0x80FF,
+    0x00FA, 0x80EB, 0x00EE, 0x00E4, 0x80E1, 0x00A0, 0x80A5,
+    0x80AF, 0x00AA, 0x80BB, 0x00BE, 0x00B4, 0x80B1, 0x8093,
+    0x0096, 0x009C, 0x8099, 0x0088, 0x808D, 0x8087, 0x0082,
+    0x8183, 0x0186, 0x018C, 0x8189, 0x0198, 0x819D, 0x8197,
+    0x0192, 0x01B0, 0x81B5, 0x81BF, 0x01BA, 0x81AB, 0x01AE,
+    0x01A4, 0x81A1, 0x01E0, 0x81E5, 0x81EF, 0x01EA, 0x81FB,
+    0x01FE, 0x01F4, 0x81F1, 0x81D3, 0x01D6, 0x01DC, 0x81D9,
+    0x01C8, 0x81CD, 0x81C7, 0x01C2, 0x0140, 0x8145, 0x814F,
+    0x014A, 0x815B, 0x015E, 0x0154, 0x8151, 0x8173, 0x0176,
+    0x017C, 0x8179, 0x0168, 0x816D, 0x8167, 0x0162, 0x8123,
+    0x0126, 0x012C, 0x8129, 0x0138, 0x813D, 0x8137, 0x0132,
+    0x0110, 0x8115, 0x811F, 0x011A, 0x810B, 0x010E, 0x0104,
+    0x8101, 0x8303, 0x0306, 0x030C, 0x8309, 0x0318, 0x831D,
+    0x8317, 0x0312, 0x0330, 0x8335, 0x833F, 0x033A, 0x832B,
+    0x032E, 0x0324, 0x8321, 0x0360, 0x8365, 0x836F, 0x036A,
+    0x837B, 0x037E, 0x0374, 0x8371, 0x8353, 0x0356, 0x035C,
+    0x8359, 0x0348, 0x834D, 0x8347, 0x0342, 0x03C0, 0x83C5,
+    0x83CF, 0x03CA, 0x83DB, 0x03DE, 0x03D4, 0x83D1, 0x83F3,
+    0x03F6, 0x03FC, 0x83F9, 0x03E8, 0x83ED, 0x83E7, 0x03E2,
+    0x83A3, 0x03A6, 0x03AC, 0x83A9, 0x03B8, 0x83BD, 0x83B7,
+    0x03B2, 0x0390, 0x8395, 0x839F, 0x039A, 0x838B, 0x038E,
+    0x0384, 0x8381, 0x0280, 0x8285, 0x828F, 0x028A, 0x829B,
+    0x029E, 0x0294, 0x8291, 0x82B3, 0x02B6, 0x02BC, 0x82B9,
+    0x02A8, 0x82AD, 0x82A7, 0x02A2, 0x82E3, 0x02E6, 0x02EC,
+    0x82E9, 0x02F8, 0x82FD, 0x82F7, 0x02F2, 0x02D0, 0x82D5,
+    0x82DF, 0x02DA, 0x82CB, 0x02CE, 0x02C4, 0x82C1, 0x8243,
+    0x0246, 0x024C, 0x8249, 0x0258, 0x825D, 0x8257, 0x0252,
+    0x0270, 0x8275, 0x827F, 0x027A, 0x826B, 0x026E, 0x0264,
+    0x8261, 0x0220, 0x8225, 0x822F, 0x022A, 0x823B, 0x023E,
+    0x0234, 0x8231, 0x8213, 0x0216, 0x021C, 0x8219, 0x0208,
+    0x820D, 0x8207, 0x0202 };
+
+    for(j = 0; j < data_blk_size; j++)
+    {
+        i = ((UINT16_T)(crc_accum >> 8) ^ *data_blk_ptr++) & 0xFF;
+        crc_accum = (crc_accum << 8) ^ crc_table[i];
+    }
+
+    return crc_accum;
+}
+
+void Protocol2PacketHandler::AddStuffing(UINT8_T *packet)
+{
+    int i = 0, index = 0;
+    int packet_length_in = DXL_MAKEWORD(packet[PKT_LENGTH_L], packet[PKT_LENGTH_H]);
+    int packet_length_out = packet_length_in;
+    UINT8_T temp[TXPACKET_MAX_LEN] = {0};
+
+    for(UINT8_T _s = PKT_HEADER0; _s <= PKT_LENGTH_H; _s++)
+        temp[_s] = packet[_s]; // FF FF FD XX ID LEN_L LEN_H
+    //memcpy(temp, packet, PKT_LENGTH_H+1);
+    index = PKT_INSTRUCTION;
+    for( i = 0; i < packet_length_in - 2; i++)  // except CRC
+    {
+        temp[index++] = packet[i+PKT_INSTRUCTION];
+        if(packet[i+PKT_INSTRUCTION] == 0xFD && packet[i+PKT_INSTRUCTION-1] == 0xFF && packet[i+PKT_INSTRUCTION-2] == 0xFF)
+        {   // FF FF FD
+            temp[index++] = 0xFD;
+            packet_length_out++;
+        }
+    }
+    temp[index++] = packet[PKT_INSTRUCTION+packet_length_in-2];
+    temp[index++] = packet[PKT_INSTRUCTION+packet_length_in-1];
+
+
+    //////////////////////////
+    if(packet_length_in != packet_length_out)
+        packet = (UINT8_T *)realloc(packet, index * sizeof(UINT8_T));
+
+    ///////////////////////////
+
+    for(UINT8_T _s = 0; _s < index; _s++)
+        packet[_s] = temp[_s];
+    //memcpy(packet, temp, index);
+    packet[PKT_LENGTH_L] = DXL_LOBYTE(packet_length_out);
+    packet[PKT_LENGTH_H] = DXL_HIBYTE(packet_length_out);
+}
+
+void Protocol2PacketHandler::RemoveStuffing(UINT8_T *packet)
+{
+    int i = 0, index = 0;
+    int packet_length_in = DXL_MAKEWORD(packet[PKT_LENGTH_L], packet[PKT_LENGTH_H]);
+    int packet_length_out = packet_length_in;
+
+    index = PKT_INSTRUCTION;
+    for( i = 0; i < packet_length_in - 2; i++)  // except CRC
+    {
+        if(packet[i+PKT_INSTRUCTION] == 0xFD && packet[i+PKT_INSTRUCTION+1] == 0xFD && packet[i+PKT_INSTRUCTION-1] == 0xFF && packet[i+PKT_INSTRUCTION-2] == 0xFF)
+        {   // FF FF FD FD
+            packet_length_out--;
+            i++;
+        }
+        packet[index++] = packet[i+PKT_INSTRUCTION];
+    }
+    packet[index++] = packet[PKT_INSTRUCTION+packet_length_in-2];
+    packet[index++] = packet[PKT_INSTRUCTION+packet_length_in-1];
+
+    packet[PKT_LENGTH_L] = DXL_LOBYTE(packet_length_out);
+    packet[PKT_LENGTH_H] = DXL_HIBYTE(packet_length_out);
+}
+
+int Protocol2PacketHandler::TxPacket(PortHandler *port, UINT8_T *txpacket)
+{
+    UINT16_T _total_packet_length   = 0;
+    UINT16_T _written_packet_length = 0;
+
+    if(port->is_using)
+        return COMM_PORT_BUSY;
+    port->is_using = true;
+
+    // byte stuffing for header
+    AddStuffing(txpacket);
+
+    // check max packet length
+    _total_packet_length = DXL_MAKEWORD(txpacket[PKT_LENGTH_L], txpacket[PKT_LENGTH_H]) + 7;
+                           // 7: HEADER0 HEADER1 HEADER2 RESERVED ID LENGTH_L LENGTH_H
+    if(_total_packet_length > TXPACKET_MAX_LEN)
+    {
+        port->is_using = false;
+        return COMM_TX_ERROR;
+    }
+
+    // make packet header
+    txpacket[PKT_HEADER0]   = 0xFF;
+    txpacket[PKT_HEADER1]   = 0xFF;
+    txpacket[PKT_HEADER2]   = 0xFD;
+    txpacket[PKT_RESERVED]  = 0x00;
+
+    // add CRC16
+    UINT16_T crc = UpdateCRC(0, txpacket, _total_packet_length - 2);    // 2: CRC16
+    txpacket[_total_packet_length - 2] = DXL_LOBYTE(crc);
+    txpacket[_total_packet_length - 1] = DXL_HIBYTE(crc);
+
+    // tx packet
+    port->ClearPort();
+    _written_packet_length = port->WritePort(txpacket, _total_packet_length);
+    if(_total_packet_length != _written_packet_length)
+    {
+        port->is_using = false;
+        return COMM_TX_FAIL;
+    }
+
+    return COMM_SUCCESS;
+}
+
+int Protocol2PacketHandler::RxPacket(PortHandler *port, UINT8_T *rxpacket)
+{
+    int     _result         = COMM_TX_FAIL;
+
+    UINT16_T _rx_length     = 0;
+    UINT16_T _wait_length   = 11;
+            // minimum length ( HEADER0 HEADER1 HEADER2 RESERVED ID LENGTH_L LENGTH_H INST ERROR CRC16_L CRC16_H )
+
+    while(true)
+    {
+        _rx_length += port->ReadPort(&rxpacket[_rx_length], _wait_length - _rx_length);
+        if(_rx_length >= _wait_length)
+        {
+            UINT16_T _idx = 0;
+
+            // find packet header
+//            for(_idx = 0; _idx < (_rx_length - 2); _idx++)
+//            {
+//                if(rxpacket[_idx] == 0xFF && rxpacket[_idx+1] == 0xFF && rxpacket[_idx+2] == 0xFD)
+//                    break;
+//            }
+            for(_idx = 0; _idx < (_rx_length - 3); _idx++)
+            {
+                if((rxpacket[_idx] == 0xFF) && (rxpacket[_idx+1] == 0xFF) && (rxpacket[_idx+2] == 0xFD) && (rxpacket[_idx+3] != 0xFD))
+                    break;
+            }
+
+            if(_idx == 0)   // found at the beginning of the packet
+            {
+                // re-calculate the exact length of the rx packet
+                _wait_length = DXL_MAKEWORD(rxpacket[PKT_LENGTH_L], rxpacket[PKT_LENGTH_H]) + PKT_LENGTH_H + 1;
+                if(_wait_length > 100)
+                	fprintf(stderr, "id : %d , _wait_length : %d \n", rxpacket[PKT_ID], _wait_length);
+
+				if(_wait_length > RXPACKET_MAX_LEN - 10)
+				{
+					_result = COMM_RX_CORRUPT;
+				    break;
+				}
+
+                if(_rx_length < _wait_length)
+                {
+                    // check timeout
+                    if(port->IsPacketTimeout() == true)
+                    {
+                        if(_rx_length == 0)
+                            _result = COMM_RX_TIMEOUT;
+                        else {
+                            _result = COMM_RX_CORRUPT;
+//                        	fprintf(stderr, "Fail Rx Packet(short_length1) : ");
+//                        	for(int _i = 0; _i < _rx_length + 11; _i++)
+//                        	{
+//                        		fprintf(stderr, " %x", rxpacket[_i]);
+//                        	}
+//                        	fprintf(stderr, "\n");
+                        }
+                        break;
+                    }
+
+                    else
+                        continue;
+                }
+
+                // verify CRC16
+                UINT16_T crc = DXL_MAKEWORD(rxpacket[_wait_length-2], rxpacket[_wait_length-1]);
+                if(UpdateCRC(0, rxpacket, _wait_length - 2) == crc)
+                    _result = COMM_SUCCESS;
+                else {
+                    _result = COMM_RX_CORRUPT;
+//                	fprintf(stderr, "Rx Packet(crc_error) : ");
+//                	for(int _i = 0; _i < _rx_length + 11; _i++)
+//                	{
+//                		fprintf(stderr, " %x", rxpacket[_i]);
+//                	}
+//                	fprintf(stderr, "\n");
+                }
+                break;
+            }
+            else
+            {
+                // remove unnecessary packets
+                for(UINT8_T _s = 0; _s < _rx_length - _idx; _s++)
+                    rxpacket[_s] = rxpacket[_idx + _s];
+                //memcpy(&rxpacket[0], &rxpacket[_idx], _rx_length - _idx);
+                _rx_length -= _idx;
+            }
+        }
+        else
+        {
+            // check timeout
+            if(port->IsPacketTimeout() == true)
+            {
+                if(_rx_length == 0)
+                    _result = COMM_RX_TIMEOUT;
+                else {
+                    _result = COMM_RX_CORRUPT;
+//                	fprintf(stderr, "Fail Rx Packet(short_length2): ");
+//                	for(int _i = 0; _i < _rx_length + 11; _i++)
+//                	{
+//                		fprintf(stderr, " %x", rxpacket[_i]);
+//                	}
+//                	fprintf(stderr, "\n");
+                }
+                break;
+            }
+        }
+    }
+    port->is_using = false;
+
+    if(_result == COMM_SUCCESS)
+        RemoveStuffing(rxpacket);
+
+    return _result;
+}
+
+// NOT for BulkRead / SyncRead instruction
+int Protocol2PacketHandler::TxRxPacket(PortHandler *port, UINT8_T *txpacket, UINT8_T *rxpacket, UINT8_T *error)
+{
+    int _result = COMM_TX_FAIL;
+
+    // tx packet
+    _result = TxPacket(port, txpacket);
+    if(_result != COMM_SUCCESS)
+        return _result;
+
+    // (ID == Broadcast ID && NOT BulkRead) == no need to wait for status packet
+    // (Instruction == Action) == no need to wait for status packet
+    if((txpacket[PKT_ID] == BROADCAST_ID && txpacket[PKT_INSTRUCTION] != INST_BULK_READ) ||
+       (txpacket[PKT_ID] == BROADCAST_ID && txpacket[PKT_INSTRUCTION] != INST_SYNC_READ) ||
+       (txpacket[PKT_INSTRUCTION] == INST_ACTION))
+    {
+        port->is_using = false;
+        return _result;
+    }
+
+    // set packet timeout
+    if(txpacket[PKT_INSTRUCTION] == INST_READ)
+        port->SetPacketTimeout((UINT16_T)(DXL_MAKEWORD(txpacket[PKT_PARAMETER0+2], txpacket[PKT_PARAMETER0+3]) + 11));
+    else
+        port->SetPacketTimeout((UINT16_T)11);   // HEADER0 HEADER1 HEADER2 RESERVED ID LENGTH_L LENGTH_H INST ERROR CRC16_L CRC16_H
+
+    // rx packet
+    _result = RxPacket(port, rxpacket);
+    // TODO: check txpacket ID == rxpacket ID
+
+    if(_result == COMM_SUCCESS && txpacket[PKT_ID] != BROADCAST_ID)
+    {
+        if(error != 0)
+            *error = (UINT8_T)rxpacket[PKT_ERROR];
+    }
+
+    return _result;
+}
+
+int Protocol2PacketHandler::Ping(PortHandler *port, UINT8_T id, UINT8_T *error)
+{
+    return Ping(port, id, 0, error);
+}
+
+int Protocol2PacketHandler::Ping(PortHandler *port, UINT8_T id, UINT16_T *model_number, UINT8_T *error)
+{
+    int _result                 = COMM_TX_FAIL;
+
+    UINT8_T txpacket[10]        = {0};
+    UINT8_T rxpacket[14]        = {0};
+
+    if(id >= BROADCAST_ID)
+        return COMM_NOT_AVAILABLE;
+
+    txpacket[PKT_ID]            = id;
+    txpacket[PKT_LENGTH_L]      = 3;
+    txpacket[PKT_LENGTH_H]      = 0;
+    txpacket[PKT_INSTRUCTION]   = INST_PING;
+
+    _result = TxRxPacket(port, txpacket, rxpacket, error);
+    if(_result == COMM_SUCCESS && model_number != 0)
+        *model_number = DXL_MAKEWORD(rxpacket[PKT_PARAMETER0+1], rxpacket[PKT_PARAMETER0+2]);
+
+    return _result;
+}
+
+int Protocol2PacketHandler::BroadcastPing(PortHandler *port, std::vector<UINT8_T> &id_list)
+{
+    const int STATUS_LENGTH     = 14;
+    int _result                 = COMM_TX_FAIL;
+
+    id_list.clear();
+
+    UINT16_T _rx_length         = 0;
+    UINT16_T _wait_length       = STATUS_LENGTH * MAX_ID;
+
+    UINT8_T txpacket[10]        = {0};
+    UINT8_T rxpacket[STATUS_LENGTH * MAX_ID] = {0};
+
+    txpacket[PKT_ID]            = BROADCAST_ID;
+    txpacket[PKT_LENGTH_L]      = 3;
+    txpacket[PKT_LENGTH_H]      = 0;
+    txpacket[PKT_INSTRUCTION]   = INST_PING;
+
+    _result = TxPacket(port, txpacket);
+    if(_result != COMM_SUCCESS)
+    {
+        port->is_using = false;
+        return _result;
+    }
+
+    // set rx timeout
+    port->SetPacketTimeout((UINT16_T)(_wait_length * 30));
+
+    while(1)
+    {
+        _rx_length += port->ReadPort(&rxpacket[_rx_length], _wait_length - _rx_length);
+        if(port->IsPacketTimeout() == true)// || _rx_length >= _wait_length)
+            break;
+    }
+
+    port->is_using = false;
+
+    if(_rx_length == 0)
+        return COMM_RX_TIMEOUT;
+
+    while(1)
+    {
+        if(_rx_length < STATUS_LENGTH)
+            return COMM_RX_CORRUPT;
+
+        UINT16_T _idx = 0;
+
+        // find packet header
+        for(_idx = 0; _idx < (_rx_length - 2); _idx++)
+        {
+            if(rxpacket[_idx] == 0xFF && rxpacket[_idx+1] == 0xFF && rxpacket[_idx+2] == 0xFD)
+                break;
+        }
+
+        if(_idx == 0)   // found at the beginning of the packet
+        {
+            // verify CRC16
+            UINT16_T crc = DXL_MAKEWORD(rxpacket[STATUS_LENGTH-2], rxpacket[STATUS_LENGTH-1]);
+
+            if(UpdateCRC(0, rxpacket, STATUS_LENGTH - 2) == crc)
+            {
+                _result = COMM_SUCCESS;
+
+                id_list.push_back(rxpacket[PKT_ID]);
+
+                for(UINT8_T _s = 0; _s < _rx_length - STATUS_LENGTH; _s++)
+                    rxpacket[_s] = rxpacket[STATUS_LENGTH + _s];
+                _rx_length -= STATUS_LENGTH;
+
+                if(_rx_length == 0)
+                    return _result;
+            }
+            else
+            {
+                _result = COMM_RX_CORRUPT;
+
+                // remove header (0xFF 0xFF 0xFD)
+                for(UINT8_T _s = 0; _s < _rx_length - 3; _s++)
+                    rxpacket[_s] = rxpacket[3 + _s];
+                _rx_length -= 3;
+            }
+        }
+        else
+        {
+            // remove unnecessary packets
+            for(UINT8_T _s = 0; _s < _rx_length - _idx; _s++)
+                rxpacket[_s] = rxpacket[_idx + _s];
+            _rx_length -= _idx;
+        }
+    }
+
+    return _result;
+}
+
+int Protocol2PacketHandler::Action(PortHandler *port, UINT8_T id)
+{
+    int _result                 = COMM_TX_FAIL;
+
+    UINT8_T txpacket[10]        = {0};
+
+    txpacket[PKT_ID]            = id;
+    txpacket[PKT_LENGTH_L]      = 3;
+    txpacket[PKT_LENGTH_H]      = 0;
+    txpacket[PKT_INSTRUCTION]   = INST_ACTION;
+
+    return TxRxPacket(port, txpacket, 0);
+}
+
+int Protocol2PacketHandler::Reboot(PortHandler *port, UINT8_T id, UINT8_T *error)
+{
+    UINT8_T txpacket[10]        = {0};
+    UINT8_T rxpacket[11]        = {0};
+
+    txpacket[PKT_ID]            = id;
+    txpacket[PKT_LENGTH_L]      = 3;
+    txpacket[PKT_LENGTH_H]      = 0;
+    txpacket[PKT_INSTRUCTION]   = INST_REBOOT;
+
+    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};
+    UINT8_T rxpacket[11]        = {0};
+
+    txpacket[PKT_ID]            = id;
+    txpacket[PKT_LENGTH_L]      = 4;
+    txpacket[PKT_LENGTH_H]      = 0;
+    txpacket[PKT_INSTRUCTION]   = INST_FACTORY_RESET;
+    txpacket[PKT_PARAMETER0]    = option;
+
+    return TxRxPacket(port, txpacket, rxpacket, error);
+}
+
+int Protocol2PacketHandler::ReadTx(PortHandler *port, UINT8_T id, UINT16_T address, UINT16_T length)
+{
+    int _result                 = COMM_TX_FAIL;
+
+    UINT8_T txpacket[14]        = {0};
+
+    if(id >= BROADCAST_ID)
+        return COMM_NOT_AVAILABLE;
+
+    txpacket[PKT_ID]            = id;
+    txpacket[PKT_LENGTH_L]      = 7;
+    txpacket[PKT_LENGTH_H]      = 0;
+    txpacket[PKT_INSTRUCTION]   = INST_READ;
+    txpacket[PKT_PARAMETER0+0]  = (UINT8_T)DXL_LOBYTE(address);
+    txpacket[PKT_PARAMETER0+1]  = (UINT8_T)DXL_HIBYTE(address);
+    txpacket[PKT_PARAMETER0+2]  = (UINT8_T)DXL_LOBYTE(length);
+    txpacket[PKT_PARAMETER0+3]  = (UINT8_T)DXL_HIBYTE(length);
+
+    _result = TxPacket(port, txpacket);
+
+    // set packet timeout
+    if(_result == COMM_SUCCESS)
+        port->SetPacketTimeout((UINT16_T)(length + 11));
+
+    return _result;
+}
+
+int Protocol2PacketHandler::ReadRx(PortHandler *port, UINT16_T length, UINT8_T *data, UINT8_T *error)
+{
+    int _result                 = COMM_TX_FAIL;
+    //UINT8_T *rxpacket           = (UINT8_T *)malloc(RXPACKET_MAX_LEN);//(length + 11 + (length/3));  // (length/3): consider stuffing
+    UINT8_T *rxpacket           = ( UINT8_T* )calloc(RXPACKET_MAX_LEN, sizeof(UINT8_T));//(length + 11 + (length/3));  // (length/3): consider stuffing
+    //UINT8_T *rxpacket           = new UINT8_T[length + 11 + (length/3)];    // (length/3): consider stuffing
+
+    _result = RxPacket(port, rxpacket);
+    if(_result == COMM_SUCCESS)
+    {
+        if(error != 0)
+            *error = (UINT8_T)rxpacket[PKT_ERROR];
+        for(UINT8_T _s = 0; _s < length; _s++)
+            data[_s] = rxpacket[PKT_PARAMETER0 + 1 + _s];
+        //memcpy(data, &rxpacket[PKT_PARAMETER0+1], length);
+    }
+//    else
+//    {
+//    	fprintf(stderr, "Fail Rx Packet : ");
+//    	for(int _i = 0; _i < length + 11; _i++)
+//    	{
+//    		fprintf(stderr, " %x", rxpacket[_i]);
+//    	}
+//    	fprintf(stderr, "\n");
+//    }
+
+    free(rxpacket);
+    //delete[] rxpacket;
+    return _result;
+}
+
+int Protocol2PacketHandler::ReadTxRx(PortHandler *port, UINT8_T id, UINT16_T address, UINT16_T length, UINT8_T *data, UINT8_T *error)
+{
+    int _result                 = COMM_TX_FAIL;
+
+    _result = ReadTx(port, id, address, length);
+    if(_result != COMM_SUCCESS)
+        return _result;
+
+    return ReadRx(port, length, data, error);
+}
+
+int Protocol2PacketHandler::Read1ByteTx(PortHandler *port, UINT8_T id, UINT16_T address)
+{
+    return ReadTx(port, id, address, 1);
+}
+int Protocol2PacketHandler::Read1ByteRx(PortHandler *port, UINT8_T *data, UINT8_T *error)
+{
+    UINT8_T _data[1] = {0};
+    int _result = ReadRx(port, 1, _data, error);
+    *data = _data[0];
+    return _result;
+}
+int Protocol2PacketHandler::Read1ByteTxRx(PortHandler *port, UINT8_T id, UINT16_T address, UINT8_T *data, UINT8_T *error)
+{
+    int _result = COMM_TX_FAIL;
+
+    _result = Read1ByteTx(port, id, address);
+    if(_result != COMM_SUCCESS)
+        return _result;
+
+    return Read1ByteRx(port, data, error);
+}
+
+int Protocol2PacketHandler::Read2ByteTx(PortHandler *port, UINT8_T id, UINT16_T address)
+{
+    return ReadTx(port, id, address, 2);
+}
+int Protocol2PacketHandler::Read2ByteRx(PortHandler *port, UINT16_T *data, UINT8_T *error)
+{
+    UINT8_T _data[2] = {0};
+    int _result = ReadRx(port, 2, _data, error);
+    *data = DXL_MAKEWORD(_data[0], _data[1]);
+    return _result;
+}
+int Protocol2PacketHandler::Read2ByteTxRx(PortHandler *port, UINT8_T id, UINT16_T address, UINT16_T *data, UINT8_T *error)
+{
+    int _result = COMM_TX_FAIL;
+
+    _result = Read2ByteTx(port, id, address);
+    if(_result != COMM_SUCCESS)
+        return _result;
+
+    return Read2ByteRx(port, data, error);
+}
+
+int Protocol2PacketHandler::Read4ByteTx(PortHandler *port, UINT8_T id, UINT16_T address)
+{
+    return ReadTx(port, id, address, 4);
+}
+int Protocol2PacketHandler::Read4ByteRx(PortHandler *port, UINT32_T *data, UINT8_T *error)
+{
+    UINT8_T _data[4] = {0};
+    int _result = ReadRx(port, 4, _data, error);
+    *data = DXL_MAKEDWORD(DXL_MAKEWORD(_data[0], _data[1]), DXL_MAKEWORD(_data[2], _data[3]));
+    return _result;
+}
+int Protocol2PacketHandler::Read4ByteTxRx(PortHandler *port, UINT8_T id, UINT16_T address, UINT32_T *data, UINT8_T *error)
+{
+    int _result = COMM_TX_FAIL;
+
+    _result = Read4ByteTx(port, id, address);
+    if(_result != COMM_SUCCESS)
+        return _result;
+
+    return Read4ByteRx(port, data, error);
+}
+
+
+int Protocol2PacketHandler::WriteTxOnly(PortHandler *port, UINT8_T id, UINT16_T address, UINT16_T length, UINT8_T *data)
+{
+    int _result                 = COMM_TX_FAIL;
+
+    UINT8_T *txpacket           = (UINT8_T *)malloc(length+12);
+    //UINT8_T *txpacket           = new UINT8_T[length+12];
+
+    txpacket[PKT_ID]            = id;
+    txpacket[PKT_LENGTH_L]      = DXL_LOBYTE(length+5);
+    txpacket[PKT_LENGTH_H]      = DXL_HIBYTE(length+5);
+    txpacket[PKT_INSTRUCTION]   = INST_WRITE;
+    txpacket[PKT_PARAMETER0+0]  = (UINT8_T)DXL_LOBYTE(address);
+    txpacket[PKT_PARAMETER0+1]  = (UINT8_T)DXL_HIBYTE(address);
+
+    for(UINT8_T _s = 0; _s < length; _s++)
+        txpacket[PKT_PARAMETER0+2+_s] = data[_s];
+    //memcpy(&txpacket[PKT_PARAMETER0+2], data, length);
+
+    _result = TxPacket(port, txpacket);
+    port->is_using = false;
+
+    free(txpacket);
+    //delete[] txpacket;
+    return _result;
+}
+
+int Protocol2PacketHandler::WriteTxRx(PortHandler *port, UINT8_T id, UINT16_T address, UINT16_T length, UINT8_T *data, UINT8_T *error)
+{
+    int _result                 = COMM_TX_FAIL;
+
+    UINT8_T *txpacket           = (UINT8_T *)malloc(length + 12);
+    //UINT8_T *txpacket           = new UINT8_T[length+12];
+    UINT8_T rxpacket[11]        = {0};
+
+    txpacket[PKT_ID]            = id;
+    txpacket[PKT_LENGTH_L]      = DXL_LOBYTE(length+5);
+    txpacket[PKT_LENGTH_H]      = DXL_HIBYTE(length+5);
+    txpacket[PKT_INSTRUCTION]   = INST_WRITE;
+    txpacket[PKT_PARAMETER0+0]  = (UINT8_T)DXL_LOBYTE(address);
+    txpacket[PKT_PARAMETER0+1]  = (UINT8_T)DXL_HIBYTE(address);
+
+    for(UINT8_T _s = 0; _s < length; _s++)
+        txpacket[PKT_PARAMETER0+2+_s] = data[_s];
+    //memcpy(&txpacket[PKT_PARAMETER0+2], data, length);
+
+    _result = TxRxPacket(port, txpacket, rxpacket, error);
+
+    free(txpacket);
+    //delete[] txpacket;
+    return _result;
+}
+
+int Protocol2PacketHandler::Write1ByteTxOnly(PortHandler *port, UINT8_T id, UINT16_T address, UINT8_T data)
+{
+    UINT8_T _data[1] = { data };
+    return WriteTxOnly(port, id, address, 1, _data);
+}
+int Protocol2PacketHandler::Write1ByteTxRx(PortHandler *port, UINT8_T id, UINT16_T address, UINT8_T data, UINT8_T *error)
+{
+    UINT8_T _data[1] = { data };
+    return WriteTxRx(port, id, address, 1, _data, error);
+}
+
+int Protocol2PacketHandler::Write2ByteTxOnly(PortHandler *port, UINT8_T id, UINT16_T address, UINT16_T data)
+{
+    UINT8_T _data[2] = { DXL_LOBYTE(data), DXL_HIBYTE(data) };
+    return WriteTxOnly(port, id, address, 2, _data);
+}
+int Protocol2PacketHandler::Write2ByteTxRx(PortHandler *port, UINT8_T id, UINT16_T address, UINT16_T data, UINT8_T *error)
+{
+    UINT8_T _data[2] = { DXL_LOBYTE(data), DXL_HIBYTE(data) };
+    return WriteTxRx(port, id, address, 2, _data, error);
+}
+
+int Protocol2PacketHandler::Write4ByteTxOnly(PortHandler *port, UINT8_T id, UINT16_T address, UINT32_T data)
+{
+    UINT8_T _data[4] = { DXL_LOBYTE(DXL_LOWORD(data)), DXL_HIBYTE(DXL_LOWORD(data)), DXL_LOBYTE(DXL_HIWORD(data)), DXL_HIBYTE(DXL_HIWORD(data)) };
+    return WriteTxOnly(port, id, address, 4, _data);
+}
+int Protocol2PacketHandler::Write4ByteTxRx(PortHandler *port, UINT8_T id, UINT16_T address, UINT32_T data, UINT8_T *error)
+{
+    UINT8_T _data[4] = { DXL_LOBYTE(DXL_LOWORD(data)), DXL_HIBYTE(DXL_LOWORD(data)), DXL_LOBYTE(DXL_HIWORD(data)), DXL_HIBYTE(DXL_HIWORD(data)) };
+    return WriteTxRx(port, id, address, 4, _data, error);
+}
+
+int Protocol2PacketHandler::RegWriteTxOnly(PortHandler *port, UINT8_T id, UINT16_T address, UINT16_T length, UINT8_T *data)
+{
+    int _result                 = COMM_TX_FAIL;
+
+    UINT8_T *txpacket           = (UINT8_T *)malloc(length + 12);
+    //UINT8_T *txpacket           = new UINT8_T[length+12];
+
+    txpacket[PKT_ID]            = id;
+    txpacket[PKT_LENGTH_L]      = DXL_LOBYTE(length+5);
+    txpacket[PKT_LENGTH_H]      = DXL_HIBYTE(length+5);
+    txpacket[PKT_INSTRUCTION]   = INST_REG_WRITE;
+    txpacket[PKT_PARAMETER0+0]  = (UINT8_T)DXL_LOBYTE(address);
+    txpacket[PKT_PARAMETER0+1]  = (UINT8_T)DXL_HIBYTE(address);
+
+    for(UINT8_T _s = 0; _s < length; _s++)
+        txpacket[PKT_PARAMETER0+2+_s] = data[_s];
+    //memcpy(&txpacket[PKT_PARAMETER0+2], data, length);
+
+    _result = TxPacket(port, txpacket);
+    port->is_using = false;
+
+    free(txpacket);
+    //delete[] txpacket;
+    return _result;
+}
+
+int Protocol2PacketHandler::RegWriteTxRx(PortHandler *port, UINT8_T id, UINT16_T address, UINT16_T length, UINT8_T *data, UINT8_T *error)
+{
+    int _result                 = COMM_TX_FAIL;
+
+    UINT8_T *txpacket           = (UINT8_T *)malloc(length + 12);
+    //UINT8_T *txpacket           = new UINT8_T[length+12];
+    UINT8_T rxpacket[11]        = {0};
+
+    txpacket[PKT_ID]            = id;
+    txpacket[PKT_LENGTH_L]      = DXL_LOBYTE(length+5);
+    txpacket[PKT_LENGTH_H]      = DXL_HIBYTE(length+5);
+    txpacket[PKT_INSTRUCTION]   = INST_REG_WRITE;
+    txpacket[PKT_PARAMETER0+0]  = (UINT8_T)DXL_LOBYTE(address);
+    txpacket[PKT_PARAMETER0+1]  = (UINT8_T)DXL_HIBYTE(address);
+
+    for(UINT8_T _s = 0; _s < length; _s++)
+        txpacket[PKT_PARAMETER0+2+_s] = data[_s];
+    //memcpy(&txpacket[PKT_PARAMETER0+2], data, length);
+
+    _result = TxRxPacket(port, txpacket, rxpacket, error);
+
+    free(txpacket);
+    //delete[] txpacket;
+    return _result;
+}
+
+int Protocol2PacketHandler::SyncReadTx(PortHandler *port, UINT16_T start_address, UINT16_T data_length, UINT8_T *param, UINT16_T param_length)
+{
+    int _result                 = COMM_TX_FAIL;
+
+    UINT8_T *txpacket           = (UINT8_T *)malloc(param_length + 14);
+            // 14: HEADER0 HEADER1 HEADER2 RESERVED ID LEN_L LEN_H INST START_ADDR_L START_ADDR_H DATA_LEN_L DATA_LEN_H CRC16_L CRC16_H
+
+    txpacket[PKT_ID]            = BROADCAST_ID;
+    txpacket[PKT_LENGTH_L]      = DXL_LOBYTE(param_length + 7); // 7: INST START_ADDR_L START_ADDR_H DATA_LEN_L DATA_LEN_H CRC16_L CRC16_H
+    txpacket[PKT_LENGTH_H]      = DXL_HIBYTE(param_length + 7); // 7: INST START_ADDR_L START_ADDR_H DATA_LEN_L DATA_LEN_H CRC16_L CRC16_H
+    txpacket[PKT_INSTRUCTION]   = INST_SYNC_READ;
+    txpacket[PKT_PARAMETER0+0]  = DXL_LOBYTE(start_address);
+    txpacket[PKT_PARAMETER0+1]  = DXL_HIBYTE(start_address);
+    txpacket[PKT_PARAMETER0+2]  = DXL_LOBYTE(data_length);
+    txpacket[PKT_PARAMETER0+3]  = DXL_HIBYTE(data_length);
+
+    for(UINT8_T _s = 0; _s < param_length; _s++)
+        txpacket[PKT_PARAMETER0+4+_s] = param[_s];
+    //memcpy(&txpacket[PKT_PARAMETER0+4], param, param_length);
+
+    _result = TxPacket(port, txpacket);
+    if(_result == COMM_SUCCESS)
+        port->SetPacketTimeout((UINT16_T)((11 + data_length) * param_length));
+
+    free(txpacket);
+    return _result;
+}
+
+int Protocol2PacketHandler::SyncWriteTxOnly(PortHandler *port, UINT16_T start_address, UINT16_T data_length, UINT8_T *param, UINT16_T param_length)
+{
+    int _result                 = COMM_TX_FAIL;
+
+    UINT8_T *txpacket           = (UINT8_T *)malloc(param_length + 14);
+    //UINT8_T *txpacket           = new UINT8_T[param_length + 14];
+            // 14: HEADER0 HEADER1 HEADER2 RESERVED ID LEN_L LEN_H INST START_ADDR_L START_ADDR_H DATA_LEN_L DATA_LEN_H CRC16_L CRC16_H
+
+    txpacket[PKT_ID]            = BROADCAST_ID;
+    txpacket[PKT_LENGTH_L]      = DXL_LOBYTE(param_length + 7); // 7: INST START_ADDR_L START_ADDR_H DATA_LEN_L DATA_LEN_H CRC16_L CRC16_H
+    txpacket[PKT_LENGTH_H]      = DXL_HIBYTE(param_length + 7); // 7: INST START_ADDR_L START_ADDR_H DATA_LEN_L DATA_LEN_H CRC16_L CRC16_H
+    txpacket[PKT_INSTRUCTION]   = INST_SYNC_WRITE;
+    txpacket[PKT_PARAMETER0+0]  = DXL_LOBYTE(start_address);
+    txpacket[PKT_PARAMETER0+1]  = DXL_HIBYTE(start_address);
+    txpacket[PKT_PARAMETER0+2]  = DXL_LOBYTE(data_length);
+    txpacket[PKT_PARAMETER0+3]  = DXL_HIBYTE(data_length);
+
+    for(UINT8_T _s = 0; _s < param_length; _s++)
+        txpacket[PKT_PARAMETER0+4+_s] = param[_s];
+    //memcpy(&txpacket[PKT_PARAMETER0+4], param, param_length);
+
+    _result = TxRxPacket(port, txpacket, 0, 0);
+
+    free(txpacket);
+    //delete[] txpacket;
+    return _result;
+}
+
+int Protocol2PacketHandler::BulkReadTx(PortHandler *port, UINT8_T *param, UINT16_T param_length)
+{
+    int _result                 = COMM_TX_FAIL;
+
+    UINT8_T *txpacket           = (UINT8_T *)malloc(param_length + 10);
+    //UINT8_T *txpacket           = new UINT8_T[param_length + 10];
+            // 10: HEADER0 HEADER1 HEADER2 RESERVED ID LEN_L LEN_H INST CRC16_L CRC16_H
+
+    txpacket[PKT_ID]            = BROADCAST_ID;
+    txpacket[PKT_LENGTH_L]      = DXL_LOBYTE(param_length + 3); // 3: INST CRC16_L CRC16_H
+    txpacket[PKT_LENGTH_H]      = DXL_HIBYTE(param_length + 3); // 3: INST CRC16_L CRC16_H
+    txpacket[PKT_INSTRUCTION]   = INST_BULK_READ;
+
+    for(UINT8_T _s = 0; _s < param_length; _s++)
+        txpacket[PKT_PARAMETER0+_s] = param[_s];
+    //memcpy(&txpacket[PKT_PARAMETER0], param, param_length);
+
+    _result = TxPacket(port, txpacket);
+    if(_result == COMM_SUCCESS)
+    {
+        int _wait_length = 0;
+        for(int _i = 0; _i < param_length; _i += 5)
+            _wait_length += DXL_MAKEWORD(param[_i+3], param[_i+4]) + 10;
+        port->SetPacketTimeout((UINT16_T)_wait_length);
+    }
+
+    free(txpacket);
+    //delete[] txpacket;
+    return _result;
+}
+
+int Protocol2PacketHandler::BulkWriteTxOnly(PortHandler *port, UINT8_T *param, UINT16_T param_length)
+{
+    int _result                 = COMM_TX_FAIL;
+
+    UINT8_T *txpacket           = (UINT8_T *)malloc(param_length + 10);
+    //UINT8_T *txpacket           = new UINT8_T[param_length + 10];
+            // 10: HEADER0 HEADER1 HEADER2 RESERVED ID LEN_L LEN_H INST CRC16_L CRC16_H
+
+    txpacket[PKT_ID]            = BROADCAST_ID;
+    txpacket[PKT_LENGTH_L]      = DXL_LOBYTE(param_length + 3); // 3: INST CRC16_L CRC16_H
+    txpacket[PKT_LENGTH_H]      = DXL_HIBYTE(param_length + 3); // 3: INST CRC16_L CRC16_H
+    txpacket[PKT_INSTRUCTION]   = INST_BULK_WRITE;
+
+    for(UINT8_T _s = 0; _s < param_length; _s++)
+        txpacket[PKT_PARAMETER0+_s] = param[_s];
+    //memcpy(&txpacket[PKT_PARAMETER0], param, param_length);
+
+    _result = TxRxPacket(port, txpacket, 0, 0);
+
+    free(txpacket);
+    //delete[] txpacket;
+    return _result;
+}
--- a/dynamixel_sdk/src/dynamixel_sdk_linux/PortHandlerLinux.cpp
+++ b/dynamixel_sdk/src/dynamixel_sdk_linux/PortHandlerLinux.cpp
@@ -0,0 +1,253 @@
+/*
+ * PortHandlerLinux.cpp
+ *
+ *  Created on: 2016. 1. 26.
+ *      Author: zerom
+ */
+
+#include <stdio.h>
+#include <fcntl.h>
+#include <string.h>
+#include <unistd.h>
+#include <termios.h>
+#include <time.h>
+#include <sys/time.h>
+#include <sys/ioctl.h>
+#include <linux/serial.h>
+
+#include "dynamixel_sdk_linux/PortHandlerLinux.h"
+
+#define LATENCY_TIMER   4  // msec (USB latency timer)
+
+using namespace ROBOTIS;
+
+PortHandlerLinux::PortHandlerLinux(const char *port_name)
+    : socket_fd_(-1),
+      baudrate_(DEFAULT_BAUDRATE),
+      packet_start_time_(0.0),
+      packet_timeout_(0.0),
+      tx_time_per_byte(0.0)
+{
+    is_using = false;
+    SetPortName(port_name);
+}
+
+bool PortHandlerLinux::OpenPort()
+{
+    return SetBaudRate(baudrate_);
+}
+
+void PortHandlerLinux::ClosePort()
+{
+    if(socket_fd_ != -1)
+        close(socket_fd_);
+    socket_fd_ = -1;
+}
+
+void PortHandlerLinux::ClearPort()
+{
+    tcflush(socket_fd_, TCIOFLUSH);
+}
+
+void PortHandlerLinux::SetPortName(const char *port_name)
+{
+    strcpy(port_name_, port_name);
+}
+
+char *PortHandlerLinux::GetPortName()
+{
+    return port_name_;
+}
+
+// TODO: baud number ??
+bool PortHandlerLinux::SetBaudRate(const int baudrate)
+{
+    int _baud = GetCFlagBaud(baudrate);
+
+    ClosePort();
+
+    if(_baud <= 0)   // custom baudrate
+    {
+        SetupPort(B38400);
+        baudrate_ = baudrate;
+        return SetCustomBaudrate(baudrate);
+    }
+    else
+    {
+        baudrate_ = baudrate;
+        return SetupPort(_baud);
+    }
+
+    return true;
+}
+
+int PortHandlerLinux::GetBaudRate()
+{
+    return baudrate_;
+}
+
+int PortHandlerLinux::GetBytesAvailable()
+{
+    int _bytes_available;
+    ioctl(socket_fd_, FIONREAD, &_bytes_available);
+    return _bytes_available;
+}
+
+int PortHandlerLinux::ReadPort(UINT8_T *packet, int length)
+{
+    return read(socket_fd_, packet, length);
+}
+
+int PortHandlerLinux::WritePort(UINT8_T *packet, int length)
+{
+    return write(socket_fd_, packet, length);
+}
+
+void PortHandlerLinux::SetPacketTimeout(UINT16_T packet_length)
+{
+    packet_start_time_  = GetCurrentTime();
+    packet_timeout_     = (tx_time_per_byte * (double)packet_length) + (LATENCY_TIMER * 2.0) + 2.0;
+}
+
+void PortHandlerLinux::SetPacketTimeout(double msec)
+{
+    packet_start_time_  = GetCurrentTime();
+    packet_timeout_     = msec;
+}
+
+bool PortHandlerLinux::IsPacketTimeout()
+{
+    if(GetTimeSinceStart() > packet_timeout_)
+    {
+        packet_timeout_ = 0;
+        return true;
+    }
+    return false;
+}
+
+double PortHandlerLinux::GetCurrentTime()
+{
+//    struct timeval _tv;
+//
+//    gettimeofday(&_tv, 0);
+//
+//    return ((double)_tv.tv_sec * 1000.0 + (double)_tv.tv_usec / 1000.0);
+
+	struct timespec _tv;
+	clock_gettime( CLOCK_REALTIME, &_tv);
+	return ((double)_tv.tv_sec*1000.0 + (double)_tv.tv_nsec*0.001*0.001);
+}
+
+double PortHandlerLinux::GetTimeSinceStart()
+{
+    double _time;
+
+    _time = GetCurrentTime() - packet_start_time_;
+    if(_time < 0.0)
+        packet_start_time_ = GetCurrentTime();
+
+    return _time;
+}
+
+bool PortHandlerLinux::SetupPort(int cflag_baud)
+{
+    struct termios newtio;
+
+    socket_fd_ = open(port_name_, O_RDWR|O_NOCTTY|O_NONBLOCK);
+    if(socket_fd_ < 0)
+    {
+        printf("[PortHandlerLinux::SetupPort] Error opening serial port!\n");
+        return false;
+    }
+
+    bzero(&newtio, sizeof(newtio)); // clear struct for new port settings
+
+    newtio.c_cflag = cflag_baud | CS8 | CLOCAL | CREAD;
+    newtio.c_iflag = IGNPAR;
+    newtio.c_oflag      = 0;
+    newtio.c_lflag      = 0;
+    newtio.c_cc[VTIME]  = 0;
+    newtio.c_cc[VMIN]   = 0;
+
+    // clean the buffer and activate the settings for the port
+    tcflush(socket_fd_, TCIFLUSH);
+    tcsetattr(socket_fd_, TCSANOW, &newtio);
+
+    tx_time_per_byte = (1000.0 / (double)baudrate_) * 10.0;
+    return true;
+}
+
+bool PortHandlerLinux::SetCustomBaudrate(int speed)
+{
+    // try to set a custom divisor
+    struct serial_struct ss;
+    if(ioctl(socket_fd_, TIOCGSERIAL, &ss) != 0)
+    {
+        printf("[PortHandlerLinux::SetCustomBaudrate] TIOCGSERIAL failed!\n");
+        return false;
+    }
+
+    ss.flags = (ss.flags & ~ASYNC_SPD_MASK) | ASYNC_SPD_CUST;
+    ss.custom_divisor = (ss.baud_base + (speed / 2)) / speed;
+    int closest_speed = ss.baud_base / ss.custom_divisor;
+
+    if(closest_speed < speed * 98 / 100 || closest_speed > speed * 102 / 100)
+    {
+        printf("[PortHandlerLinux::SetCustomBaudrate] Cannot set speed to %d, closest is %d \n", speed, closest_speed);
+        return false;
+    }
+
+    if(ioctl(socket_fd_, TIOCSSERIAL, &ss) < 0)
+    {
+        printf("[PortHandlerLinux::SetCustomBaudrate] TIOCSSERIAL failed!\n");
+        return false;
+    }
+
+    tx_time_per_byte = (1000.0 / (double)speed) * 10.0;
+    return true;
+}
+
+int PortHandlerLinux::GetCFlagBaud(int baudrate)
+{
+    switch(baudrate)
+    {
+    case 9600:
+        return B9600;
+    case 19200:
+        return B19200;
+    case 38400:
+        return B38400;
+    case 57600:
+        return B57600;
+    case 115200:
+        return B115200;
+    case 230400:
+        return B230400;
+    case 460800:
+        return B460800;
+    case 500000:
+        return B500000;
+    case 576000:
+        return B576000;
+    case 921600:
+        return B921600;
+    case 1000000:
+        return B1000000;
+    case 1152000:
+        return B1152000;
+    case 1500000:
+        return B1500000;
+    case 2000000:
+        return B2000000;
+    case 2500000:
+        return B2500000;
+    case 3000000:
+        return B3000000;
+    case 3500000:
+        return B3500000;
+    case 4000000:
+        return B4000000;
+    default:
+        return -1;
+    }
+}