diff --git a/dynamixel_base/include/dynamixel_common.h b/dynamixel_base/include/dynamixel_common.h
index ea125c550f3593b27a54b4f6469a947ac5f14446..7090a876973b125ccfa0017e4b694cadf6ed7e8f 100644
--- a/dynamixel_base/include/dynamixel_common.h
+++ b/dynamixel_base/include/dynamixel_common.h
@@ -11,6 +11,7 @@
 #define     DYN_NO_DEVICE       2
 #define     DYN_TIMEOUT         3
 #define     DYN_COMM_ERROR      4
+#define     DYN_BUSY            5
 
 typedef struct{
   unsigned char *data_addr;
diff --git a/dynamixel_base/src/dynamixel_master.c b/dynamixel_base/src/dynamixel_master.c
index 62e202cc3f14488f04f8196f624fe335b40236f6..cfeb073cfbbfdce3f57a719c8efebf0beea2dd13 100644
--- a/dynamixel_base/src/dynamixel_master.c
+++ b/dynamixel_base/src/dynamixel_master.c
@@ -133,6 +133,29 @@ unsigned char dyn_master_wait_transmission(TDynamixelMaster *master)
     return DYN_COMM_ERROR;
 }
 
+unsigned char dyn_master_is_transmission_done(TDynamixelMaster *master)
+{
+  unsigned char error;
+
+  error=comm_is_send_done(master->comm_dev);
+  if(error==COMM_BUSY)
+    return DYN_BUSY;
+  else if(error==COMM_SUCCESS)
+  {
+    if(master->comm_dev->time!=0x00000000)
+    {
+      // start the new timeout
+      time_set_timeout(master->comm_dev->time,master->rx_timeout_ms*1000);
+    }
+    return DYN_SUCCESS;
+  }
+  else
+  {
+    master->set_rx_mode();
+    return DYN_COMM_ERROR;
+  }
+}
+
 unsigned char dyn_master_send(TDynamixelMaster *master)
 {
   unsigned char error;
@@ -192,6 +215,429 @@ unsigned char dyn_master_wait_reception(TDynamixelMaster *master)
   }
 }
 
+unsigned char dyn_master_is_reception_done(TDynamixelMaster *master)
+{
+  if(!master->packet_ready)
+  {
+    if(master->comm_dev->time!=0x00000000)
+    {
+      if(time_is_timeout(master->comm_dev->time))
+      {
+        comm_cancel_irq_receive(master->comm_dev);
+        comm_cancel_dma_receive(master->comm_dev);
+        master->received_bytes=0x00;
+        return DYN_TIMEOUT;
+      }
+      else
+        return DYN_BUSY;
+    }
+    else
+      return DYN_BUSY;
+  }
+  else
+  {
+    master->packet_ready=0x00;
+    return DYN_SUCCESS;
+  }
+}
+
+unsigned char dyn_master_start_read_table(TDynamixelMaster *master,unsigned char id,unsigned short int address,unsigned short int length,unsigned char *data)
+{
+  unsigned char error;
+
+  // generate the read packet for the desired device
+  if(master->version==DYN_VER1)
+    dyn_init_read_packet(master->tx_buffer,id,address,length);
+  else
+    dyn2_init_read_packet(master->tx_buffer,id,address,length);
+  master->rx_num_packets=0x01;
+  if(master->return_level==no_return || id==DYN_BROADCAST_ID)
+    master->rx_no_answer=0x01;
+  else
+    master->rx_no_answer=0x00;
+  // enable transmission
+  master->set_tx_mode();
+  // send the data
+  if((error=dyn_master_send(master))!=DYN_SUCCESS)
+    master->set_rx_mode();
+
+  return error;
+}
+
+unsigned char dyn_master_is_read_table_done(TDynamixelMaster *master,unsigned char id,unsigned short int address,unsigned short int length,unsigned char *data)
+{
+  unsigned char error;
+
+  if((error=dyn_master_is_transmission_done(master))==DYN_SUCCESS)
+  {
+    // wait for the replay within the given timeout
+    if(master->return_level!=no_return && id!=DYN_BROADCAST_ID)
+    {
+      if((error=dyn_master_is_reception_done(master))==DYN_SUCCESS)
+      {
+        // check the input packet checksum
+        if(master->version==DYN_VER1)
+        {
+          if(dyn_check_checksum(master->rx_buffer)==0xFF)
+          {
+            if((error=dyn_get_status_error(master->rx_buffer))==DYN_SUCCESS)
+            {
+              if(dyn_get_read_status_data(master->rx_buffer,data)!=length)// not enough data
+                return DYN_INST_ERROR;
+              else
+                return DYN_SUCCESS;
+            }
+            else 
+              return error;
+          }
+          else
+            return DYN_CHECKSUM_ERROR;
+        }
+        else
+        {
+          if(dyn2_check_checksum(master->rx_buffer)==0x01)
+          {
+            if((error=dyn2_get_status_error(master->rx_buffer))==DYN_SUCCESS)
+            {
+              if(dyn2_get_read_status_data(master->rx_buffer,data)!=length)// not enough data
+                return DYN_INST_ERROR;
+              else
+                return DYN_SUCCESS;
+            }
+            else
+              return error;
+          }
+          else
+            return DYN_CHECKSUM_ERROR;
+        }
+      }
+      else
+        return error;
+    }
+    else
+      return DYN_SUCCESS;
+  }
+  else
+    return error;
+}
+
+unsigned char dyn_master_start_write_table(TDynamixelMaster *master,unsigned char id,unsigned short int address,unsigned short int length,unsigned char *data)
+{
+  unsigned char error;
+
+  // generate the read packet for the desired device
+  if(master->version==DYN_VER1)
+    dyn_init_write_packet(master->tx_buffer,id,address,length,data);
+  else
+    dyn2_init_write_packet(master->tx_buffer,id,address,length,data);
+  master->rx_num_packets=0x01;
+  if(master->return_level==return_all && id!=DYN_BROADCAST_ID)
+    master->rx_no_answer=0x00;
+  else
+    master->rx_no_answer=0x01;
+  // enable transmission
+  master->set_tx_mode();
+  // send the data
+  if((error=dyn_master_send(master))!=DYN_SUCCESS)
+    master->set_rx_mode();
+
+  return error;
+}
+
+unsigned char dyn_master_is_write_table_done(TDynamixelMaster *master,unsigned char id,unsigned short int address,unsigned short int length,unsigned char *data)
+{
+  unsigned char error;
+
+  if((error=dyn_master_is_transmission_done(master))==DYN_SUCCESS)
+  {
+    // wait for the replay within the given timeout
+    if(master->return_level==return_all && id!=DYN_BROADCAST_ID)
+    {
+      if((error=dyn_master_is_reception_done(master))==DYN_SUCCESS)
+      {
+        // check the input packet checksum
+        if(master->version==DYN_VER1)
+        {
+          if(dyn_check_checksum(master->rx_buffer)==0xFF)
+            return dyn_get_status_error(master->rx_buffer);
+          else
+            return DYN_CHECKSUM_ERROR;
+        }
+        else
+        {
+          if(dyn2_check_checksum(master->rx_buffer)==0x01)
+            return dyn2_get_status_error(master->rx_buffer);
+          else
+            return DYN_CHECKSUM_ERROR;
+        }
+      }
+      else
+        return error;
+    }
+    else
+      return DYN_SUCCESS;
+  }
+  else
+    return error;
+}
+
+unsigned char dyn_master_start_sync_write(TDynamixelMaster *master,unsigned char num,unsigned char *ids,unsigned short int address, unsigned short int length, TWriteData *data)
+{
+  unsigned char error;
+
+  // generate the read packet for the desired device
+  if(master->version==DYN_VER1)
+    dyn_init_sync_write_packet(master->tx_buffer,num,ids,address,length,data);
+  else
+    dyn2_init_sync_write_packet(master->tx_buffer,num,ids,address,length,data);
+  master->rx_num_packets=0x01;
+  master->rx_no_answer=0x00;
+  // enable transmission
+  master->set_tx_mode();
+  // send the data
+  if((error=dyn_master_send(master))!=DYN_SUCCESS)
+    master->set_rx_mode();
+
+  return error;
+}
+
+unsigned char dyn_master_is_sync_write_done(TDynamixelMaster *master,unsigned char num,unsigned char *ids,unsigned short int address, unsigned short int length, TWriteData *data)
+{
+  unsigned char error;
+
+  error=comm_is_send_done(master->comm_dev);
+  if(error==COMM_BUSY)
+    return DYN_BUSY;
+  else if(error==COMM_SUCCESS)
+    return DYN_SUCCESS;
+  else
+  {
+    master->set_rx_mode();
+    return DYN_COMM_ERROR;
+  }
+}
+
+unsigned char dyn_master_start_sync_read(TDynamixelMaster *master,unsigned char num,unsigned char *ids,unsigned short int address, unsigned short int length, TWriteData *data)
+{
+  unsigned char error;
+
+  // generate the read packet for the desired device
+  if(master->version==DYN_VER2)
+  {
+    dyn2_init_sync_read_packet(master->tx_buffer,num,ids,address,length);
+    master->rx_num_packets=num;
+    if(master->return_level==no_return)
+      master->rx_no_answer=0x01;
+    else
+      master->rx_no_answer=0x00;
+    // enable transmission
+    master->set_tx_mode();
+    // send the data
+    if((error=dyn_master_send(master))!=DYN_SUCCESS)
+      master->set_rx_mode();
+
+    return error;
+  }
+  else
+    return DYN_INST_ERROR;
+}
+
+unsigned char dyn_master_is_sync_read_done(TDynamixelMaster *master,unsigned char num,unsigned char *ids,unsigned short int address, unsigned short int length, TWriteData *data)
+{
+  static unsigned char num_dev_done=0;
+  unsigned char error;
+
+  // generate the read packet for the desired device
+  if(master->version==DYN_VER2)
+  {
+    if((error=dyn_master_is_transmission_done(master))==DYN_SUCCESS)
+    {
+      // wait for the replay within the given timeout
+      if(master->return_level!=no_return)
+      {
+        if(num_dev_done<num)
+        {
+          if((error=dyn_master_is_reception_done(master))==DYN_SUCCESS)
+          {
+            if(dyn2_check_checksum(master->rx_buffer)==0x01)
+            {
+              if((error=dyn2_get_status_error(master->rx_buffer))==DYN_SUCCESS)
+              {
+                while(dyn2_get_read_status_id(master->rx_buffer)!=ids[num_dev_done]) num_dev_done++;
+                if(dyn2_get_read_status_data(master->rx_buffer,data[num_dev_done].data_addr)!=length)// not enough data
+                  return DYN_INST_ERROR;
+                else
+                {
+                  num_dev_done++;
+                  return DYN_BUSY;
+                }
+              }
+              else
+                return error;
+            }
+            else
+              return DYN_CHECKSUM_ERROR;
+          }
+          else
+            return error;
+        }
+        else 
+          return DYN_SUCCESS;
+      }
+      else
+        return DYN_SUCCESS;
+    }
+    else
+      return error;
+  }
+  else
+    return DYN_INST_ERROR;
+}
+
+unsigned char dyn_master_start_bulk_write(TDynamixelMaster *master,unsigned char num,unsigned char *ids,unsigned short int *address, unsigned short int *length, TWriteData *data)
+{
+  unsigned char error;
+
+  // generate the read packet for the desired device
+  if(master->version==DYN_VER2)
+  {
+    dyn2_init_bulk_write_packet(master->tx_buffer,num,ids,address,length,data);
+    master->rx_num_packets=0x01;
+    master->rx_no_answer=0x00;
+    // enable transmission
+    master->set_tx_mode();
+    // send the data
+    if((error=dyn_master_send(master))!=DYN_SUCCESS)
+      master->set_rx_mode();
+
+    return error;
+  }
+  else
+    return DYN_INST_ERROR;
+}
+
+unsigned char dyn_master_is_bulk_write_done(TDynamixelMaster *master,unsigned char num,unsigned char *ids,unsigned short int *address, unsigned short int *length, TWriteData *data)
+{
+  unsigned char error;
+
+  if(master->version==DYN_VER2)
+  {
+    error=comm_is_send_done(master->comm_dev);
+    if(error==COMM_BUSY)
+      return DYN_BUSY;
+    else if(error==COMM_SUCCESS)
+      return DYN_SUCCESS;
+    else
+    {
+      master->set_rx_mode();
+      return DYN_COMM_ERROR;
+    }
+  }
+  else
+    return DYN_INST_ERROR;
+}
+
+unsigned char dyn_master_start_bulk_read(TDynamixelMaster *master,unsigned char num,unsigned char *ids,unsigned short int *address, unsigned short int *length, TWriteData *data)
+{
+  unsigned char error,i,ver1_address[255],ver1_length[255];
+
+  // generate the read packet for the desired device
+  if(master->version==DYN_VER1)
+  {
+    for(i=0;i<num;i++)
+    {
+      ver1_address[i]=address[i];
+      ver1_length[i]=length[i];
+    }
+    dyn_init_bulk_read_packet(master->tx_buffer,num,ids,ver1_address,ver1_length);
+  }
+  else
+    dyn2_init_bulk_read_packet(master->tx_buffer,num,ids,address,length);
+  master->rx_num_packets=num;
+  if(master->return_level==no_return)
+    master->rx_no_answer=0x01;
+  else
+    master->rx_no_answer=0x00;
+  // enable transmission
+  master->set_tx_mode();
+  // send the data
+  if((error=dyn_master_send(master))!=DYN_SUCCESS)
+    master->set_rx_mode();
+
+  return error;
+}
+
+unsigned char dyn_master_is_bulk_read_done(TDynamixelMaster *master,unsigned char num,unsigned char *ids,unsigned short int *address, unsigned short int *length, TWriteData *data)
+{
+  static unsigned char num_dev_done=0;
+  unsigned char error;
+
+  if((error=dyn_master_is_transmission_done(master))==DYN_SUCCESS)
+  {
+    // wait for the replay within the given timeout
+    if(master->return_level!=no_return)
+    {
+      if(num_dev_done<num)
+      {
+        if((error=dyn_master_is_reception_done(master))==DYN_SUCCESS)
+        {
+          // check the input packet checksum
+          if(master->version==DYN_VER1)
+          {
+            if(dyn_check_checksum(master->rx_buffer)==0xFF)
+            {
+              if((error=dyn_get_status_error(master->rx_buffer))==DYN_SUCCESS)
+              {
+                while(dyn_get_read_status_id(master->rx_buffer)!=ids[num_dev_done]) num_dev_done++;
+                if(dyn_get_read_status_data(master->rx_buffer,data[num_dev_done].data_addr)!=length[num_dev_done])// not enough data
+                  return DYN_INST_ERROR;
+                else
+                {
+                  num_dev_done++;
+                  return DYN_BUSY;
+                }
+              }
+              else
+                return error;
+            }
+            else
+              return DYN_CHECKSUM_ERROR;
+          }
+          else
+          {
+            if(dyn2_check_checksum(master->rx_buffer)==0x01)
+            {
+              if((error=dyn2_get_status_error(master->rx_buffer))==DYN_SUCCESS)
+              {
+                while(dyn2_get_read_status_id(master->rx_buffer)!=ids[num_dev_done]) num_dev_done++;
+                if(dyn2_get_read_status_data(master->rx_buffer,data[num_dev_done].data_addr)!=length[num_dev_done])// not enough data
+                  return DYN_INST_ERROR;
+                else
+                {
+                  num_dev_done++;
+                  return DYN_BUSY;
+                }
+              }
+              else
+                return error;
+            }
+            else
+              return DYN_CHECKSUM_ERROR;
+          }
+        }
+        else
+          return error;
+      }
+      else
+        return DYN_SUCCESS;
+    }
+    else
+      return DYN_SUCCESS;
+  }
+  else
+    return error;
+}
+
 /* public functions */
 void dyn_master_init(TDynamixelMaster *master,TComm *dev,TDynVersion version)
 {
@@ -574,6 +1020,7 @@ unsigned char dyn_master_bulk_read(TDynamixelMaster *master,unsigned char num,un
       {
         if(master->version==DYN_VER1)
         {
+          while(dyn_get_read_status_id(master->rx_buffer)!=ids[i]) i++;
           if(dyn_get_read_status_data(master->rx_buffer,data[i].data_addr)!=length[i])// not enough data
             error=DYN_INST_ERROR;
         }
diff --git a/dynamixel_manager/Makefile b/dynamixel_manager/Makefile
new file mode 100755
index 0000000000000000000000000000000000000000..ab1e59b5b6dea777f90b49ffc73ebf51ef73e345
--- /dev/null
+++ b/dynamixel_manager/Makefile
@@ -0,0 +1,85 @@
+# setup
+# modified by zerom for WinARM 8/2010
+
+COMPILE_OPTS = -mlittle-endian -mthumb -mthumb-interwork
+COMPILE_OPTS += -Wall -O2 -fno-common
+#COMPILE_OPTS += -Wall -g -fno-common
+COMPILE_OPTS += -ffreestanding -nostdlib
+
+COMPILE_OPTS_M4_FPU = -mfloat-abi=hard -mfpu=fpv4-sp-d16 -mcpu=cortex-m4
+COMPILE_OPTS_M0 = -mfloat-abi=softfp -mcpu=cortex-m0
+COMPILE_OPTS_M0plus = -mfloat-abi=softfp -mcpu=cortex-m0plus
+COMPILE_OPTS_M3 = -mfloat-abi=softfp -mcpu=cortex-m3
+
+COMM_PATH = ../comm
+UTILS_PATH = ../utils
+DYN_BASE_PATH = ../dynamixel_base
+
+INCLUDE_DIRS = -I./include/ -I$(COMM_PATH)/include -I$(UTILS_PATH)/include -I$(DYN_BASE_PATH)/include
+
+TCHAIN_PREFIX=arm-none-eabi-
+
+CC = $(TCHAIN_PREFIX)gcc
+CFLAGS = $(COMPILE_OPTS) $(INCLUDE_DIRS)
+
+AR = $(TCHAIN_PREFIX)ar
+ARFLAGS = cr
+
+DYNAMIXEL_OUT_M4_FPU = ./lib/dynamixel_manager_m4_fpu.a
+DYNAMIXEL_OUT_M0 = ./lib/dynamixel_manager_m0.a
+DYNAMIXEL_OUT_M0plus = ./lib/dynamixel_manager_m0plus.a
+DYNAMIXEL_OUT_M3 = ./lib/dynamixel_manager_m3.a
+
+SRC_DIR=./src/
+SRC=$(wildcard $(SRC_DIR)*.c)
+
+DYNAMIXEL_M4_FPU_OBJ_DIR=build/m4_fpu/
+DYNAMIXEL_M4_FPU_OBJS_TMP = $(notdir $(SRC:.c=.o))
+DYNAMIXEL_M4_FPU_OBJS = $(patsubst %,$(DYNAMIXEL_M4_FPU_OBJ_DIR)%,$(DYNAMIXEL_M4_FPU_OBJS_TMP))
+
+DYNAMIXEL_M0_OBJ_DIR=build/m0/
+DYNAMIXEL_M0_OBJS_TMP = $(notdir $(SRC:.c=.o))
+DYNAMIXEL_M0_OBJS = $(patsubst %,$(DYNAMIXEL_M0_OBJ_DIR)%,$(DYNAMIXEL_M0_OBJS_TMP))
+
+DYNAMIXEL_M0plus_OBJ_DIR=build/m0plus/
+DYNAMIXEL_M0plus_OBJS_TMP = $(notdir $(SRC:.c=.o))
+DYNAMIXEL_M0plus_OBJS = $(patsubst %,$(DYNAMIXEL_M0plus_OBJ_DIR)%,$(DYNAMIXEL_M0plus_OBJS_TMP))
+
+DYNAMIXEL_M3_OBJ_DIR=build/m3/
+DYNAMIXEL_M3_OBJS_TMP = $(notdir $(SRC:.c=.o))
+DYNAMIXEL_M3_OBJS = $(patsubst %,$(DYNAMIXEL_M3_OBJ_DIR)%,$(DYNAMIXEL_M3_OBJS_TMP))
+
+all: $(DYNAMIXEL_OUT_M4_FPU) $(DYNAMIXEL_OUT_M0) $(DYNAMIXEL_OUT_M0plus) $(DYNAMIXEL_OUT_M3)
+$(DYNAMIXEL_M4_FPU_OBJ_DIR)%.o: $(SRC_DIR)%.c
+	$(CC) -c $(CFLAGS) $(COMPILE_OPTS_M4_FPU) -o $@ $<
+$(DYNAMIXEL_M0_OBJ_DIR)%.o: $(SRC_DIR)%.c
+	$(CC) -c $(CFLAGS) $(COMPILE_OPTS_M0) -o $@ $<
+$(DYNAMIXEL_M0plus_OBJ_DIR)%.o: $(SRC_DIR)%.c
+	$(CC) -c $(CFLAGS) $(COMPILE_OPTS_M0plus) -o $@ $<
+$(DYNAMIXEL_M3_OBJ_DIR)%.o: $(SRC_DIR)%.c
+	$(CC) -c $(CFLAGS) $(COMPILE_OPTS_M3) -o $@ $<
+mkdir_build: 
+	mkdir -p build/m4_fpu
+	mkdir -p build/m0
+	mkdir -p build/m0plus
+	mkdir -p build/m3
+$(DYNAMIXEL_OUT_M4_FPU): mkdir_build $(DYNAMIXEL_M4_FPU_OBJS)
+	mkdir -p lib
+	$(AR) $(ARFLAGS) $@ $(DYNAMIXEL_M4_FPU_OBJS)
+$(DYNAMIXEL_OUT_M0): mkdir_build $(DYNAMIXEL_M0_OBJS)
+	mkdir -p lib
+	$(AR) $(ARFLAGS) $@ $(DYNAMIXEL_M0_OBJS)
+$(DYNAMIXEL_OUT_M0plus): mkdir_build $(DYNAMIXEL_M0plus_OBJS)
+	mkdir -p lib
+	$(AR) $(ARFLAGS) $@ $(DYNAMIXEL_M0plus_OBJS)
+$(DYNAMIXEL_OUT_M3): mkdir_build $(DYNAMIXEL_M3_OBJS)
+	mkdir -p lib
+	$(AR) $(ARFLAGS) $@ $(DYNAMIXEL_M3_OBJS)
+
+clean:
+	rm -f $(DYNAMIXEL_M4_FPU_OBJS)
+	rm -f $(DYNAMIXEL_M0_OBJS)
+	rm -f $(DYNAMIXEL_M0plus_OBJS)
+	rm -f $(DYNAMIXEL_M3_OBJS)
+	rm -rf lib
+	rm -rf build
diff --git a/dynamixel_manager/include/dyn_devices.h b/dynamixel_manager/include/dyn_devices.h
new file mode 100644
index 0000000000000000000000000000000000000000..c10582ff5b9f007c2e6aaf3538290bacaa46489d
--- /dev/null
+++ b/dynamixel_manager/include/dyn_devices.h
@@ -0,0 +1,27 @@
+#ifndef _DYN_DEVICES_H
+#define _DYN_DEVICES_H
+
+// servo modules
+#define       SERVO_DX113     0x0071
+#define       SERVO_DX116     0x0074
+#define       SERVO_DX117     0x0075
+#define       SERVO_AX12A     0x000C
+#define       SERVO_AX12W     0x012C
+#define       SERVO_AX18A     0x0012
+#define       SERVO_RX10      0x000A
+#define       SERVO_MX12W     0x0168
+#define       SERVO_MX28      0x001D
+#define       SERVO_RX24F     0x0018
+#define       SERVO_RX28      0x001C
+#define       SERVO_RX64      0x0040
+#define       SERVO_MX64      0x0136
+#define       SERVO_EX106     0x006B
+#define       SERVO_MX106     0x0140
+#define       SERVO_XL320     0x015E
+
+#define       IS_SERVO(model) (model==SERVO_DX113 || model==SERVO_DX116 || model==SERVO_DX117 || model==SERVO_AX12A \
+                               model==SERVO_AX12W || model==SERVO_AX18A || model==SERVO_RX10 || model==SERVO_MX12W \
+                               model==SERVO_MX28 || model==SERVO_RX24F || model==SERVO_RX28 || model==SERVO_RX64 \
+                               model==SERVO_MX64 || model==SERVO_EX106 || model==SERVO_MX106 || model==SERVO_XL320)
+
+#endif
diff --git a/dynamixel_manager/include/dyn_manager.h b/dynamixel_manager/include/dyn_manager.h
new file mode 100644
index 0000000000000000000000000000000000000000..67e2d7416bb671173daca67b165cc83cbae22e76
--- /dev/null
+++ b/dynamixel_manager/include/dyn_manager.h
@@ -0,0 +1,24 @@
+#ifndef _DYN_MANAGER_H
+#define _DYN_MANAGER_H
+
+#include "dynamixel_master.h"
+#include "dyn_module.h"
+
+#define    DYN_MANAGER_PERIODIC            0xFF
+
+#define    DYN_MANAGER_READ                  0
+#define    DYN_MANAGER_WRITE                 1
+
+// public functions
+void dyn_manager_init(void);
+void dyn_manager_scan(void);
+void dyn_manager_add_master(TDynamixelMaster *master);
+unsigned char dyn_manager_get_num_masters(void);
+void dyn_manager_add_module(TDynModule *module);
+unsigned char dyn_manager_get_num_modules(void);
+void dyn_manager_reg_single_op(unsigned char op,unsigned char id,unsigned short int address,unsigned short int length,unsigned char *data,unsigned char rep);
+void dyn_manager_reg_sync_op(unsigned char op,unsigned char num,unsigned char *ids,unsigned short int address, unsigned short int length, TWriteData *data,unsigned char rep);
+void dyn_manager_reg_bulk_op(unsigned char op,unsigned char num,unsigned char *ids,unsigned short int *address, unsigned short int *length, TWriteData *data,unsigned char rep);
+void dyn_manager_loop(void);
+
+#endif 
diff --git a/dynamixel_manager/include/dyn_module.h b/dynamixel_manager/include/dyn_module.h
new file mode 100644
index 0000000000000000000000000000000000000000..9942ddc24c996b75d651e3afd6cf86d361125511
--- /dev/null
+++ b/dynamixel_manager/include/dyn_module.h
@@ -0,0 +1,22 @@
+#ifndef _DYN_MODULE_H
+#define _DYN_MODULE_H
+
+#define     MODULE_MAX_NUM_MODELS     32
+
+typedef struct{
+  unsigned short int models[MODULE_MAX_NUM_MODELS];
+  unsigned char num_models;
+  unsigned char period_count;
+  void (*init)(void);
+  void (*add_device)(unsigned char id,unsigned short int model);
+  void (*setup)(void);
+  void (*pre_process)(void);
+  void (*post_process)(void);
+}TDynModule;
+
+//public functions
+void dyn_module_init(TDynModule *module);
+void dyn_module_add_model(TDynModule *module,unsigned short int model);
+void dyn_module_add_device(TDynModule *module,unsigned char id, unsigned short int model);
+
+#endif
diff --git a/dynamixel_manager/src/dyn_manager.c b/dynamixel_manager/src/dyn_manager.c
new file mode 100644
index 0000000000000000000000000000000000000000..de9b61f7ced62cb97529d2eb2a5200d18b0db776
--- /dev/null
+++ b/dynamixel_manager/src/dyn_manager.c
@@ -0,0 +1,580 @@
+#include "dyn_manager.h"
+
+#define    DYN_MANAGER_MAX_NUM_MASTERS       4
+#define    DYN_MANAGER_MAX_NUM_MODULES       8
+#define    DYN_MANAGER_MAX_NUM_DEVICES       32
+#define    DYN_MANAGER_MAX_NUM_SINGLE_OP     16
+#define    DYN_MANAGER_MAX_NUM_SYNC_OP       4
+#define    DYN_MANAGER_MAX_NUM_BULK_OP       4
+
+extern unsigned char dyn_master_start_read_table(TDynamixelMaster *master,unsigned char id,unsigned short int address,unsigned short int length,unsigned char *data);
+extern unsigned char dyn_master_is_read_table_done(TDynamixelMaster *master,unsigned char id,unsigned short int address,unsigned short int length,unsigned char *data);
+extern unsigned char dyn_master_start_write_table(TDynamixelMaster *master,unsigned char id,unsigned short int address,unsigned short int length,unsigned char *data);
+extern unsigned char dyn_master_is_write_table_done(TDynamixelMaster *master,unsigned char id,unsigned short int address,unsigned short int length,unsigned char *data);
+extern unsigned char dyn_master_start_sync_write(TDynamixelMaster *master,unsigned char num,unsigned char *ids,unsigned short int address, unsigned short int length, TWriteData *data);
+extern unsigned char dyn_master_is_sync_write_done(TDynamixelMaster *master,unsigned char num,unsigned char *ids,unsigned short int address, unsigned short int length, TWriteData *data);
+extern unsigned char dyn_master_start_sync_read(TDynamixelMaster *master,unsigned char num,unsigned char *ids,unsigned short int address, unsigned short int length, TWriteData *data);
+extern unsigned char dyn_master_is_sync_read_done(TDynamixelMaster *master,unsigned char num,unsigned char *ids,unsigned short int address, unsigned short int length, TWriteData *data);
+extern unsigned char dyn_master_start_bulk_write(TDynamixelMaster *master,unsigned char num,unsigned char *ids,unsigned short int *address, unsigned short int *length, TWriteData *data);
+extern unsigned char dyn_master_is_bulk_write_done(TDynamixelMaster *master,unsigned char num,unsigned char *ids,unsigned short int *address, unsigned short int *length, TWriteData *data);
+extern unsigned char dyn_master_start_bulk_read(TDynamixelMaster *master,unsigned char num,unsigned char *ids,unsigned short int *address, unsigned short int *length, TWriteData *data);
+extern unsigned char dyn_master_is_bulk_read_done(TDynamixelMaster *master,unsigned char num,unsigned char *ids,unsigned short int *address, unsigned short int *length, TWriteData *data);
+
+typedef enum {start_single_ops,wait_single_ops,start_sync_ops,wait_sync_ops,start_bulk_ops,wait_bulk_ops,ops_done} dyn_manager_states_t;
+
+typedef struct{
+  unsigned char operation;
+  unsigned char id;
+  unsigned short int address;
+  unsigned short int length;
+  unsigned char *data;
+  unsigned char repetitions;
+}TDynManagerSingleOp;
+
+typedef struct{
+  unsigned char operation;
+  unsigned char num_devices;
+  unsigned char ids[DYN_MANAGER_MAX_NUM_DEVICES];
+  unsigned short int address;
+  unsigned short int length;
+  TWriteData data[DYN_MANAGER_MAX_NUM_DEVICES];
+  unsigned char repetitions;
+}TDynManagerSyncOp;
+
+typedef struct{
+  unsigned char operation;
+  unsigned char num_devices;
+  unsigned char ids[DYN_MANAGER_MAX_NUM_DEVICES];
+  unsigned short int address[DYN_MANAGER_MAX_NUM_DEVICES];
+  unsigned short int length[DYN_MANAGER_MAX_NUM_DEVICES];
+  TWriteData data[DYN_MANAGER_MAX_NUM_DEVICES];
+  unsigned char repetitions;
+}TDynManagerBulkOp;
+
+typedef struct{
+  TDynManagerSingleOp single_op[DYN_MANAGER_MAX_NUM_SINGLE_OP];
+  unsigned char num_single_op;
+  TDynManagerSyncOp sync_op[DYN_MANAGER_MAX_NUM_SYNC_OP];
+  unsigned char num_sync_op;
+  TDynManagerBulkOp bulk_op[DYN_MANAGER_MAX_NUM_BULK_OP];
+  unsigned char num_bulk_op;
+}TDynManagerOps;
+
+typedef struct{
+  unsigned short model;
+  unsigned char master_index;
+}TDynManagerDevice;
+
+typedef struct{
+  TDynamixelMaster *masters[DYN_MANAGER_MAX_NUM_MASTERS];
+  TDynManagerOps operations[DYN_MANAGER_MAX_NUM_MASTERS];
+  unsigned char num_masters;
+  TDynModule *modules[DYN_MANAGER_MAX_NUM_MODULES];
+  unsigned char num_modules;
+  TDynManagerDevice devices[DYN_MANAGER_MAX_NUM_DEVICES];
+  unsigned char num_devices;
+}TDynManager;
+
+// private variables
+TDynManager dyn_manager;
+
+// private functions
+unsigned char dyn_manager_check_id(unsigned char id)
+{
+  if(id>=DYN_MANAGER_MAX_NUM_DEVICES)
+    return 0xFF;
+  if(dyn_manager.devices[id].model==0xFFFF)
+    return 0xFF;
+  else
+    return dyn_manager.devices[id].master_index;
+}
+
+// public functions
+void dyn_manager_init(void)
+{
+  unsigned char i,j,k;
+
+  for(i=0;i<DYN_MANAGER_MAX_NUM_MASTERS;i++)
+  {
+    dyn_manager.masters[i]=0x00000000;
+    for(j=0;j<DYN_MANAGER_MAX_NUM_SINGLE_OP;j++)
+    {
+      dyn_manager.operations[i].single_op[j].operation=0xFF;
+      dyn_manager.operations[i].single_op[j].id=0xFF;
+      dyn_manager.operations[i].single_op[j].address=0x0000;
+      dyn_manager.operations[i].single_op[j].length=0x0000;
+      dyn_manager.operations[i].single_op[j].data=0x00000000;
+      dyn_manager.operations[i].single_op[j].repetitions=0x00;
+    }
+    dyn_manager.operations[i].num_single_op=0;
+    for(j=0;j<DYN_MANAGER_MAX_NUM_SYNC_OP;j++)
+    {
+      dyn_manager.operations[i].sync_op[j].operation=0xFF;
+      dyn_manager.operations[i].sync_op[j].num_devices=0;
+      for(k=0;k<DYN_MANAGER_MAX_NUM_DEVICES;k++)
+      {
+        dyn_manager.operations[i].sync_op[j].ids[k]=0xFF;
+        dyn_manager.operations[i].sync_op[j].data[k].data_addr=0x00000000;
+      }
+      dyn_manager.operations[i].sync_op[j].address=0x0000;
+      dyn_manager.operations[i].sync_op[j].length=0x0000;
+      dyn_manager.operations[i].sync_op[j].repetitions=0x00;
+    }
+    dyn_manager.operations[i].num_sync_op=0;
+    for(j=0;j<DYN_MANAGER_MAX_NUM_BULK_OP;j++)
+    {
+      dyn_manager.operations[i].bulk_op[j].operation=0xFF;
+      dyn_manager.operations[i].bulk_op[j].num_devices=0;
+      for(k=0;k<DYN_MANAGER_MAX_NUM_DEVICES;k++)
+      {
+        dyn_manager.operations[i].bulk_op[j].ids[k]=0xFF;
+        dyn_manager.operations[i].bulk_op[j].address[k]=0x0000;
+        dyn_manager.operations[i].bulk_op[j].length[k]=0x0000;
+        dyn_manager.operations[i].bulk_op[j].data[k].data_addr=0x00000000;
+      }
+      dyn_manager.operations[i].bulk_op[j].repetitions=0x00;
+    }
+    dyn_manager.operations[i].num_bulk_op=0;
+  }
+  dyn_manager.num_masters=0;
+  for(i=0;i<DYN_MANAGER_MAX_NUM_MODULES;i++)
+    dyn_manager.modules[i]=0x00000000;
+  dyn_manager.num_modules=0;
+  for(i=0;i<DYN_MANAGER_MAX_NUM_DEVICES;i++)
+  {
+    dyn_manager.devices[i].model=0xFFFF;
+    dyn_manager.devices[i].master_index=0xFF;
+  }
+  dyn_manager.num_devices=0;
+}
+
+void dyn_manager_scan(void)
+{
+  unsigned char servo_ids[DYN_MANAGER_MAX_NUM_DEVICES];
+  unsigned char i,j,k,num=0;
+  unsigned short int model;
+
+  // call all the modules init function
+  for(i=0;i<dyn_manager.num_modules;i++)
+    if(dyn_manager.modules[i]->init!=0x00000000)
+      dyn_manager.modules[i]->init();
+
+  // scan all dynamixel buses
+  for(i=0;i<dyn_manager.num_masters;i++)
+  {
+    // scan the bus
+    dyn_master_scan(dyn_manager.masters[i],&num,servo_ids);
+    for(j=0;j<num;j++)
+    {
+      // get the servo module
+      dyn_master_read_word(dyn_manager.masters[i],servo_ids[j],0x0000,&model);
+      // store internal info
+      if(dyn_manager.devices[servo_ids[j]].master_index==0xFF && dyn_manager.devices[servo_ids[j]].model==0xFFFF)// device not yet initialized
+      {
+        dyn_manager.devices[servo_ids[j]].model=model;
+        dyn_manager.devices[servo_ids[j]].master_index=i;
+        dyn_manager.num_devices++;
+        // pass the infor to each of the modules
+        for(k=0;k<dyn_manager.num_modules;k++)
+          dyn_module_add_device(dyn_manager.modules[k],servo_ids[j],model); 
+      }
+      // else ignore device
+    }
+  }
+  // configure the operations
+  for(i=0;i<dyn_manager.num_modules;i++)
+    if(dyn_manager.modules[i]->setup!=0x00000000)
+      dyn_manager.modules[i]->setup();
+}
+
+void dyn_manager_add_master(TDynamixelMaster *master)
+{
+  unsigned char i;
+
+  for(i=0;i<dyn_manager.num_masters;i++)
+    if(dyn_manager.masters[i]==master)
+      return;
+  if(i<DYN_MANAGER_MAX_NUM_MASTERS)
+  {
+    dyn_manager.masters[i]=master;
+    dyn_manager.num_masters++;
+  }
+}
+
+unsigned char dyn_manager_get_num_masters(void)
+{
+  return dyn_manager.num_masters;
+}
+
+void dyn_manager_add_module(TDynModule *module)
+{
+  unsigned char i;
+
+  for(i=0;i<dyn_manager.num_modules;i++)
+    if(dyn_manager.modules[i]==module)
+      return;
+  if(i<DYN_MANAGER_MAX_NUM_MODULES)
+  {
+    dyn_manager.modules[i]=module;
+    dyn_manager.num_modules++;
+  }
+}
+
+unsigned char dyn_manager_get_num_modules(void)
+{
+  return dyn_manager.num_modules;
+}
+
+void dyn_manager_reg_single_op(unsigned char op,unsigned char id,unsigned short int address,unsigned short int length,unsigned char *data,unsigned char rep)
+{
+  unsigned char i,master_index;
+
+  if((master_index=dyn_manager_check_id(id))==0xFF)
+    return;
+  for(i=0;i<dyn_manager.operations[master_index].num_single_op;i++)
+  {
+    if(dyn_manager.operations[master_index].single_op[i].operation==op && dyn_manager.operations[master_index].single_op[i].id==id)
+      if(dyn_manager.operations[master_index].single_op[i].address==address && dyn_manager.operations[master_index].single_op[i].length==length)
+        return;
+  }
+  if(i<DYN_MANAGER_MAX_NUM_SINGLE_OP)
+  {
+    dyn_manager.operations[master_index].single_op[i].operation=op;
+    dyn_manager.operations[master_index].single_op[i].id=id;
+    dyn_manager.operations[master_index].single_op[i].address=address;
+    dyn_manager.operations[master_index].single_op[i].length=length;
+    dyn_manager.operations[master_index].single_op[i].data=data;
+    dyn_manager.operations[master_index].single_op[i].repetitions=rep;
+    dyn_manager.operations[master_index].num_single_op++;
+  }
+}
+
+void dyn_manager_remove_single_op(unsigned char master_index,unsigned char single_op_index)
+{
+  unsigned char i;
+
+  if(master_index<dyn_manager.num_masters)
+  {
+    if(single_op_index<dyn_manager.operations[master_index].num_single_op)
+    {
+      if(single_op_index<dyn_manager.operations[master_index].num_single_op-1)
+      {
+        for(i=single_op_index;i<dyn_manager.operations[master_index].num_single_op;i++)
+        {
+          dyn_manager.operations[master_index].single_op[i].operation=dyn_manager.operations[master_index].single_op[i+1].operation;
+          dyn_manager.operations[master_index].single_op[i].id=dyn_manager.operations[master_index].single_op[i+1].id;
+          dyn_manager.operations[master_index].single_op[i].address=dyn_manager.operations[master_index].single_op[i+1].address;
+          dyn_manager.operations[master_index].single_op[i].length=dyn_manager.operations[master_index].single_op[i+1].length;
+          dyn_manager.operations[master_index].single_op[i].data=dyn_manager.operations[master_index].single_op[i+1].data;
+          dyn_manager.operations[master_index].single_op[i].repetitions=dyn_manager.operations[master_index].single_op[i+1].repetitions;
+        }
+      }
+      dyn_manager.operations[master_index].num_single_op--;
+    } 
+  }
+}
+
+void dyn_manager_reg_sync_op(unsigned char op,unsigned char num,unsigned char *ids,unsigned short int address, unsigned short int length, TWriteData *data,unsigned char rep)
+{
+  unsigned char i,j,k,master_index,present=0x00,added=0x00;
+
+  for(i=0;i<num;i++)
+  {
+    if((master_index=dyn_manager_check_id(ids[i]))!=0xFF)
+    {
+      added=0x00;
+      for(j=0;j<dyn_manager.operations[master_index].num_sync_op;j++)
+      {
+        if(dyn_manager.operations[master_index].sync_op[j].operation==op)
+        {
+          if(dyn_manager.operations[master_index].sync_op[j].address==address && dyn_manager.operations[master_index].sync_op[j].length==length)
+          {
+            present=0x00;
+            for(k=0;k<dyn_manager.operations[master_index].sync_op[i].num_devices;k++)
+            {
+              if(ids[i]==dyn_manager.operations[master_index].sync_op[j].ids[k])
+              {
+                present=0x01;
+                added=0x01;
+                break;
+              }
+            }         
+            if(present==0x00)
+            {
+              if(dyn_manager.operations[master_index].sync_op[j].num_devices<DYN_MANAGER_MAX_NUM_DEVICES)
+              {
+                dyn_manager.operations[master_index].sync_op[j].ids[dyn_manager.operations[master_index].sync_op[j].num_devices]=ids[i];
+                dyn_manager.operations[master_index].sync_op[j].data[dyn_manager.operations[master_index].sync_op[j].num_devices].data_addr=data[i].data_addr;
+                dyn_manager.operations[master_index].sync_op[j].num_devices++;     
+                added=0x01;
+              }
+            }
+          }
+        }
+      }
+      if(added==0x00)
+      {
+        if(j<DYN_MANAGER_MAX_NUM_SYNC_OP)
+        {
+          dyn_manager.operations[master_index].sync_op[j].operation=op;
+          dyn_manager.operations[master_index].sync_op[j].address=address;
+          dyn_manager.operations[master_index].sync_op[j].length=length;
+          dyn_manager.operations[master_index].sync_op[j].repetitions=rep;
+          dyn_manager.operations[master_index].sync_op[j].ids[dyn_manager.operations[master_index].sync_op[j].num_devices]=ids[i];
+          dyn_manager.operations[master_index].sync_op[j].data[dyn_manager.operations[master_index].sync_op[j].num_devices].data_addr=data[i].data_addr;
+          dyn_manager.operations[master_index].sync_op[j].num_devices++;     
+          dyn_manager.operations[master_index].num_sync_op++;
+        }  
+      }
+    }
+  }
+}
+
+void dyn_manager_remove_sync_op(unsigned char master_index,unsigned char sync_op_index)
+{
+  unsigned char i,j;
+
+  if(master_index<dyn_manager.num_masters)
+  {
+    if(sync_op_index<dyn_manager.operations[master_index].num_sync_op)
+    {
+      if(sync_op_index<dyn_manager.operations[master_index].num_sync_op-1)
+      {
+        for(i=sync_op_index;i<dyn_manager.operations[master_index].num_sync_op;i++)
+        {
+          dyn_manager.operations[master_index].sync_op[i].operation=dyn_manager.operations[master_index].sync_op[i+1].operation;
+          dyn_manager.operations[master_index].sync_op[i].address=dyn_manager.operations[master_index].sync_op[i+1].address;
+          dyn_manager.operations[master_index].sync_op[i].length=dyn_manager.operations[master_index].sync_op[i+1].length;
+          dyn_manager.operations[master_index].sync_op[i].repetitions=dyn_manager.operations[master_index].sync_op[i+1].repetitions;
+          dyn_manager.operations[master_index].sync_op[i].num_devices=dyn_manager.operations[master_index].sync_op[i+1].num_devices;
+          for(j=0;j<dyn_manager.operations[master_index].sync_op[i].num_devices;j++)
+          {
+            dyn_manager.operations[master_index].sync_op[i].ids[j]=dyn_manager.operations[master_index].sync_op[i+1].ids[j];
+            dyn_manager.operations[master_index].sync_op[i].data[j].data_addr=dyn_manager.operations[master_index].sync_op[i+1].data[j].data_addr;
+          }
+        }
+      }
+      dyn_manager.operations[master_index].num_sync_op--;
+    }
+  }
+}
+
+void dyn_manager_reg_bulk_op(unsigned char op,unsigned char num,unsigned char *ids,unsigned short int *address, unsigned short int *length, TWriteData *data,unsigned char rep)
+{
+  unsigned char i,j,k,master_index,present=0x00,added=0x00;
+
+  for(i=0;i<num;i++)
+  {
+    if((master_index=dyn_manager_check_id(ids[i]))!=0xFF)
+    {
+      added=0x00;
+      for(j=0;j<dyn_manager.operations[master_index].num_bulk_op;j++)
+      {
+        if(dyn_manager.operations[master_index].bulk_op[j].operation==op)
+        {
+          present=0x00;
+          for(k=0;k<dyn_manager.operations[master_index].bulk_op[i].num_devices;k++)
+          {
+            if(ids[i]==dyn_manager.operations[master_index].bulk_op[j].ids[k])
+            {
+              if(dyn_manager.operations[master_index].bulk_op[j].address[k]!=address[i] || dyn_manager.operations[master_index].bulk_op[j].length[k]!=length[i])
+                added=0x00;
+              else
+                added=0x01;
+              present=0x01;
+              break;
+            }
+          }
+          if(present==0x00)
+          {
+            if(dyn_manager.operations[master_index].bulk_op[j].num_devices<DYN_MANAGER_MAX_NUM_DEVICES)
+            {
+              dyn_manager.operations[master_index].bulk_op[j].ids[dyn_manager.operations[master_index].bulk_op[j].num_devices]=ids[i];
+              dyn_manager.operations[master_index].bulk_op[j].data[dyn_manager.operations[master_index].bulk_op[j].num_devices].data_addr=data[i].data_addr;
+              dyn_manager.operations[master_index].bulk_op[j].address[dyn_manager.operations[master_index].bulk_op[j].num_devices]=address[i];
+              dyn_manager.operations[master_index].bulk_op[j].length[dyn_manager.operations[master_index].bulk_op[j].num_devices]=length[i];
+              dyn_manager.operations[master_index].bulk_op[j].num_devices++;
+              added=0x01;
+            }
+          }
+        }
+      }
+      if(added==0x00)
+      {
+        if(j<DYN_MANAGER_MAX_NUM_SYNC_OP)
+        {
+          dyn_manager.operations[master_index].bulk_op[j].operation=op;
+          dyn_manager.operations[master_index].bulk_op[j].repetitions=rep;
+          dyn_manager.operations[master_index].bulk_op[j].ids[dyn_manager.operations[master_index].bulk_op[j].num_devices]=ids[i];
+          dyn_manager.operations[master_index].bulk_op[j].data[dyn_manager.operations[master_index].bulk_op[j].num_devices].data_addr=data[i].data_addr;
+          dyn_manager.operations[master_index].bulk_op[j].address[dyn_manager.operations[master_index].bulk_op[j].num_devices]=address[i];
+          dyn_manager.operations[master_index].bulk_op[j].length[dyn_manager.operations[master_index].bulk_op[j].num_devices]=length[i];
+          dyn_manager.operations[master_index].bulk_op[j].num_devices++;
+          dyn_manager.operations[master_index].num_bulk_op++;
+        }
+      }
+    }
+  }
+}
+
+void dyn_manager_remove_bulk_op(unsigned char master_index,unsigned char bulk_op_index)
+{
+  unsigned char i,j;
+
+  if(master_index<dyn_manager.num_masters)
+  {
+    if(bulk_op_index<dyn_manager.operations[master_index].num_bulk_op)
+    {
+      if(bulk_op_index<dyn_manager.operations[master_index].num_bulk_op-1)
+      {
+        for(i=bulk_op_index;i<dyn_manager.operations[master_index].num_bulk_op;i++)
+        {
+          dyn_manager.operations[master_index].bulk_op[i].operation=dyn_manager.operations[master_index].bulk_op[i+1].operation;
+          dyn_manager.operations[master_index].bulk_op[i].repetitions=dyn_manager.operations[master_index].bulk_op[i+1].repetitions;
+          dyn_manager.operations[master_index].bulk_op[i].num_devices=dyn_manager.operations[master_index].bulk_op[i+1].num_devices;
+          for(j=0;j<dyn_manager.operations[master_index].bulk_op[i].num_devices;j++)
+          {
+            dyn_manager.operations[master_index].bulk_op[i].address[j]=dyn_manager.operations[master_index].bulk_op[i+1].address[j];
+            dyn_manager.operations[master_index].bulk_op[i].length[j]=dyn_manager.operations[master_index].bulk_op[i+1].length[j];
+            dyn_manager.operations[master_index].bulk_op[i].ids[j]=dyn_manager.operations[master_index].bulk_op[i+1].ids[j];
+            dyn_manager.operations[master_index].bulk_op[i].data[j].data_addr=dyn_manager.operations[master_index].bulk_op[i+1].data[j].data_addr;
+          }
+        }
+      }
+      dyn_manager.operations[master_index].num_bulk_op--;
+    }
+  }
+}
+
+void dyn_manager_loop(void)
+{
+  unsigned char i,done,loop_done=dyn_manager.num_masters;
+  dyn_manager_states_t states[DYN_MANAGER_MAX_NUM_MASTERS];
+  unsigned char current_single_op[DYN_MANAGER_MAX_NUM_MASTERS];
+  unsigned char current_sync_op[DYN_MANAGER_MAX_NUM_MASTERS];
+  unsigned char current_bulk_op[DYN_MANAGER_MAX_NUM_MASTERS];
+  TDynManagerSingleOp *single_op[DYN_MANAGER_MAX_NUM_MASTERS];
+  TDynManagerSyncOp *sync_op[DYN_MANAGER_MAX_NUM_MASTERS];
+  TDynManagerBulkOp *bulk_op[DYN_MANAGER_MAX_NUM_MASTERS];
+
+  // initialize the remaining operations
+  for(i=0;i<dyn_manager.num_masters;i++)
+  {
+    states[i]=start_single_ops;
+    current_single_op[i]=0;
+    current_sync_op[i]=0;
+    current_bulk_op[i]=0;
+  }
+
+  // execute the pre_process functions for all modules
+  for(i=0;i<dyn_manager.num_modules;i++)
+    if(dyn_manager.modules[i]->pre_process!=0x00000000)
+      dyn_manager.modules[i]->pre_process();
+
+  // send all the commands
+  do{
+    for(i=0;i<dyn_manager.num_masters;i++)
+    {
+      switch(states[i])
+      {
+        case start_single_ops: if(current_single_op[i]>=dyn_manager.operations[i].num_single_op)
+                                 states[i]=start_sync_ops;
+                               else
+                               {
+                                 single_op[i]=&dyn_manager.operations[i].single_op[current_single_op[i]];
+                                 if(single_op[i]->operation==DYN_MANAGER_READ)
+                                   dyn_master_start_read_table(dyn_manager.masters[i],single_op[i]->id,single_op[i]->address,single_op[i]->length,single_op[i]->data);
+                                 else
+                                   dyn_master_start_write_table(dyn_manager.masters[i],single_op[i]->id,single_op[i]->address,single_op[i]->length,single_op[i]->data);
+                                 states[i]=wait_single_ops;
+                               }
+                               break;  
+        case wait_single_ops: if(single_op[i]->operation==DYN_MANAGER_READ)
+                                done=dyn_master_is_read_table_done(dyn_manager.masters[i],single_op[i]->id,single_op[i]->address,single_op[i]->length,single_op[i]->data);
+                              else
+                                done=dyn_master_is_write_table_done(dyn_manager.masters[i],single_op[i]->id,single_op[i]->address,single_op[i]->length,single_op[i]->data);
+                              if(done==DYN_BUSY)
+                                states[i]=wait_single_ops;
+                              else
+                              {
+                                if(single_op[i]->repetitions>0 && single_op[i]->repetitions!=0xFF)
+                                  single_op[i]->repetitions--;
+                                if(single_op[i]->repetitions==0)// remove operation from list 
+                                  dyn_manager_remove_single_op(i,current_single_op[i]);
+                                else
+                                  current_single_op[i]++;
+                                states[i]=start_single_ops;
+                              }
+                              break;
+        case start_sync_ops: if(current_sync_op[i]>=dyn_manager.operations[i].num_sync_op)
+                               states[i]=start_bulk_ops;
+                             else
+                             {
+                               sync_op[i]=&dyn_manager.operations[i].sync_op[current_sync_op[i]];
+                               if(sync_op[i]->operation==DYN_MANAGER_READ)
+                                 dyn_master_start_sync_read(dyn_manager.masters[i],sync_op[i]->num_devices,sync_op[i]->ids,sync_op[i]->address,sync_op[i]->length,sync_op[i]->data);
+                               else
+                                 dyn_master_start_sync_write(dyn_manager.masters[i],sync_op[i]->num_devices,sync_op[i]->ids,sync_op[i]->address,sync_op[i]->length,sync_op[i]->data);
+                               states[i]=wait_sync_ops;
+                             }
+                             break;
+        case wait_sync_ops: if(sync_op[i]->operation==DYN_MANAGER_READ)
+                              done=dyn_master_is_sync_read_done(dyn_manager.masters[i],sync_op[i]->num_devices,sync_op[i]->ids,sync_op[i]->address,sync_op[i]->length,sync_op[i]->data);
+                            else
+                              done=dyn_master_is_sync_write_done(dyn_manager.masters[i],sync_op[i]->num_devices,sync_op[i]->ids,sync_op[i]->address,sync_op[i]->length,sync_op[i]->data);
+                            if(done==DYN_BUSY)
+                              states[i]=wait_sync_ops;
+                            else
+                            {
+                              if(sync_op[i]->repetitions>0 && sync_op[i]->repetitions!=0xFF)
+                                sync_op[i]->repetitions--;
+                              if(sync_op[i]->repetitions==0)// remove operation from list 
+                                dyn_manager_remove_sync_op(i,current_sync_op[i]);
+                              else
+                                current_sync_op[i]++;
+                              states[i]=start_sync_ops;
+                            }
+                            break;
+        case start_bulk_ops: if(current_bulk_op[i]>=dyn_manager.operations[i].num_bulk_op)
+                             {
+                               states[i]=ops_done;
+                               loop_done--;
+                             }
+                             else
+                             {
+                               bulk_op[i]=&dyn_manager.operations[i].bulk_op[current_bulk_op[i]];
+                               if(bulk_op[i]->operation==DYN_MANAGER_READ)
+                                 dyn_master_start_bulk_read(dyn_manager.masters[i],bulk_op[i]->num_devices,bulk_op[i]->ids,bulk_op[i]->address,bulk_op[i]->length,bulk_op[i]->data);
+                               else
+                                 dyn_master_start_bulk_write(dyn_manager.masters[i],bulk_op[i]->num_devices,bulk_op[i]->ids,bulk_op[i]->address,bulk_op[i]->length,bulk_op[i]->data);
+                               states[i]=wait_bulk_ops;
+                             }
+                             break;
+        case wait_bulk_ops: if(bulk_op[i]->operation==DYN_MANAGER_READ)
+                              done=dyn_master_is_bulk_read_done(dyn_manager.masters[i],bulk_op[i]->num_devices,bulk_op[i]->ids,bulk_op[i]->address,bulk_op[i]->length,bulk_op[i]->data);
+                            else
+                              done=dyn_master_is_bulk_write_done(dyn_manager.masters[i],bulk_op[i]->num_devices,bulk_op[i]->ids,bulk_op[i]->address,bulk_op[i]->length,bulk_op[i]->data);
+                            if(done==DYN_BUSY)
+                              states[i]=wait_bulk_ops;
+                            else
+                            {
+                              if(bulk_op[i]->repetitions>0 && bulk_op[i]->repetitions!=0xFF)
+                                bulk_op[i]->repetitions--;
+                              if(bulk_op[i]->repetitions==0)// remove operation from list 
+                                dyn_manager_remove_bulk_op(i,current_bulk_op[i]);
+                              else
+                                current_bulk_op[i]++;
+                              states[i]=start_bulk_ops;
+                            }
+                            break;
+        case ops_done: states[i]=ops_done;
+                       break;
+      }
+    } 
+  }while(loop_done>0);
+
+  // execute the pre_process functions for all modules
+  for(i=0;i<dyn_manager.num_modules;i++)
+    if(dyn_manager.modules[i]->post_process!=0x00000000)
+      dyn_manager.modules[i]->post_process();
+}
diff --git a/dynamixel_manager/src/dyn_module.c b/dynamixel_manager/src/dyn_module.c
new file mode 100644
index 0000000000000000000000000000000000000000..7900055fd55793b86e01a3898e9daf1eebea2571
--- /dev/null
+++ b/dynamixel_manager/src/dyn_module.c
@@ -0,0 +1,45 @@
+#include "dyn_module.h"
+
+void dyn_module_init(TDynModule *module)
+{
+  unsigned char i;
+
+  module->num_models=0x00;
+  for(i=0;i<MODULE_MAX_NUM_MODELS;i++)
+    module->models[i]=0x0000;
+  module->period_count=0x00;
+  module->init=0x00000000;
+  module->add_device=0x00000000;
+  module->setup=0x00000000;
+  module->pre_process=0x00000000;
+  module->post_process=0x00000000;
+}
+
+void dyn_module_add_model(TDynModule *module,unsigned short int model)
+{
+  unsigned char i;
+
+  for(i=0;i<module->num_models;i++)
+    if(module->models[i]==model)
+      return;
+  if(i<MODULE_MAX_NUM_MODELS)
+  {
+    module->models[i]=model;
+    module->num_models++;
+  }
+}
+
+void dyn_module_add_device(TDynModule *module,unsigned char id, unsigned short int model)
+{
+  unsigned char i;
+  
+  for(i=0;i<module->num_models;i++)
+  {
+    if(model==module->models[i])
+    {
+      if(module->add_device!=0x00000000)
+        module->add_device(id,model);
+    }
+  }
+}
+
diff --git a/dynamixel_manager/test/Makefile b/dynamixel_manager/test/Makefile
new file mode 100755
index 0000000000000000000000000000000000000000..c57e9c1ab38c150a85bc5f1300b63950813da197
--- /dev/null
+++ b/dynamixel_manager/test/Makefile
@@ -0,0 +1,44 @@
+# setup
+# modified by zerom for WinARM 8/2010
+
+PROJECT_NAME=dynamixel_manager_test
+TARGET_FILES=$(wildcard *.c)
+TARGET_FILES+=$(wildcard ../src/*.c)
+TARGET_FILES+=$(wildcard ../../utils/src/*.c)
+TARGET_FILES+=$(wildcard ../../comm/src/*.c)
+BUILD_PATH=build
+BIN_PATH=bin
+
+INCLUDE_DIRS = -I../include -I../../utils/include -I../../comm/include -I../../dynamixel_base/include
+
+CC = gcc
+
+UTILS_OBJS_TMP = $(notdir $(TARGET_FILES:.c=.o))
+UTILS_OBJS = $(patsubst %,$(BUILD_PATH)/%,$(UTILS_OBJS_TMP))
+
+OUT_FILE=$(BIN_PATH)/$(PROJECT_NAME)
+
+all: $(OUT_FILE)
+
+make_dirs: 
+	mkdir -p $(BUILD_PATH)
+	mkdir -p $(BIN_PATH)
+
+$(BUILD_PATH)/%.o: %.c
+	$(CC) -c -g $(INCLUDE_DIRS) -o $@ $<
+
+$(BUILD_PATH)/%.o: ../src/%.c
+	$(CC) -c -g $(INCLUDE_DIRS) -o $@ $<
+
+$(BUILD_PATH)/%.o: ../../utils/src/%.c
+	$(CC) -c -g $(INCLUDE_DIRS) -o $@ $<
+
+$(BUILD_PATH)/%.o: ../../comm/src/%.c
+	$(CC) -c -g $(INCLUDE_DIRS) -o $@ $<
+
+$(OUT_FILE): make_dirs $(UTILS_OBJS) 
+	$(CC) -g $(UTILS_OBJS) -lpthread --output $@
+
+clean:
+	-rm -rf $(BUILD_PATH)
+	-rm -rf $(BIN_PATH)
diff --git a/dynamixel_manager/test/dyn_man_test.c b/dynamixel_manager/test/dyn_man_test.c
new file mode 100644
index 0000000000000000000000000000000000000000..e6a2fd47dc6d449c6b879b32b46bb2688e1b2005
--- /dev/null
+++ b/dynamixel_manager/test/dyn_man_test.c
@@ -0,0 +1,313 @@
+#include <stdio.h>
+#include <stdlib.h>
+#include "dyn_devices.h"
+#include "dyn_module.h"
+#include "dyn_manager.h"
+
+TDynamixelMaster dyn_master1;
+TDynamixelMaster dyn_master2;
+
+// timer functions
+unsigned long long int time_get_counts(void)
+{
+  static unsigned long long int count=0;
+ 
+  count+=10000;
+  return count;
+}
+
+unsigned int time_get_counts_per_us(void)
+{
+  return 10000;
+}
+
+// dynamixel master functions
+unsigned char dyn_master_irq_receive_cb(void *dyn_master,unsigned char byte)
+{
+
+}
+
+unsigned char dyn_master_dma_send_cb(void *dyn_master)
+{
+
+}
+
+unsigned char dyn_master_dma_receive_cb(void *dyn_master)
+{
+
+}
+
+void dummy_dyn_master_set_tx_mode(void)
+{
+
+}
+
+void dummy_dyn_master_set_rx_mode(void)
+{
+
+}
+
+void dyn_master_init(TDynamixelMaster *master,TComm *dev,TDynVersion version)
+{
+  /* assign communication functions */
+  dev->irq_receive_cb=dyn_master_irq_receive_cb;
+  dev->dma_send_cb=dyn_master_dma_send_cb;
+  dev->dma_receive_cb=dyn_master_dma_receive_cb;
+  master->comm_dev=dev;
+  master->version=version;
+  dev->data=master;
+  /* initialize the internal callbacks */
+  master->set_tx_mode=dummy_dyn_master_set_tx_mode;
+  master->set_rx_mode=dummy_dyn_master_set_rx_mode;
+  /* initialize internal variables */
+  master->packet_ready=0x00;
+  master->received_bytes=0x00;
+  master->rx_timeout_ms=50;
+  master->rx_no_answer=0x00;
+  master->rx_num_packets=0x00;
+  master->return_level=return_all;
+
+  master->set_rx_mode();
+}
+
+void dyn_master_scan(TDynamixelMaster *master,unsigned char *num,unsigned char *ids)
+{
+  unsigned int i;
+
+  if(master==&dyn_master1)
+  {
+    for(i=0;i<20;i++)
+      ids[i]=i+1;
+    (*num)=20;
+  }
+  else if(master==&dyn_master2)
+  {
+    for(i=0;i<4;i++)
+      ids[i]=i+21;
+    (*num)=4;
+  }
+}
+
+unsigned char dyn_master_read_word(TDynamixelMaster *master,unsigned char id,unsigned short int address,unsigned short int *data)
+{
+  if(master==&dyn_master1)
+  {
+    if(id>0 && id<11)
+    {
+      if(address==0x0000)
+        (*data)=SERVO_AX12A;
+    } 
+    if(id>10 && id<21)
+    {
+      if(address==0x0000)
+        (*data)=SERVO_MX28;
+    } 
+  }
+  else if(master==&dyn_master2)
+  {
+    if(id>20 && id<25)
+    {
+      if(address==0x0000)
+        (*data)=SERVO_XL320;
+    } 
+  }
+}
+
+unsigned char dyn_master_start_read_table(TDynamixelMaster *master,unsigned char id,unsigned short int address,unsigned short int length,unsigned char *data)
+{
+  unsigned short int i;
+
+  if(master==&dyn_master1)
+    printf("Master 1\n");
+  else
+    printf("Master 2\n");
+  printf("  read operation to servo %d at address %d with length %d\n",id,address,length);
+  printf("  data:\n");
+  for(i=0;i<length;i++)
+    printf("    0x%x\n",data[i]);
+
+  return DYN_SUCCESS;
+}
+
+unsigned char dyn_master_is_read_table_done(TDynamixelMaster *master,unsigned char id,unsigned short int address,unsigned short int length,unsigned char *data)
+{
+  return DYN_SUCCESS;
+}
+
+unsigned char dyn_master_start_write_table(TDynamixelMaster *master,unsigned char id,unsigned short int address,unsigned short int length,unsigned char *data)
+{
+  unsigned short int i;
+
+  if(master==&dyn_master1)
+    printf("Master 1\n");
+  else
+    printf("Master 2\n");
+  printf("  write operation to servo %d at address %d with length %d\n",id,address,length);
+  printf("  data:\n");
+  for(i=0;i<length;i++)
+    printf("    0x%x\n",data[i]);
+
+  return DYN_SUCCESS;
+}
+
+unsigned char dyn_master_is_write_table_done(TDynamixelMaster *master,unsigned char id,unsigned short int address,unsigned short int length,unsigned char *data)
+{
+  return DYN_SUCCESS;
+}
+
+unsigned char dyn_master_start_sync_write(TDynamixelMaster *master,unsigned char num,unsigned char *ids,unsigned short int address, unsigned short int length, TWriteData *data)
+{
+
+}
+
+unsigned char dyn_master_is_sync_write_done(TDynamixelMaster *master,unsigned char num,unsigned char *ids,unsigned short int address, unsigned short int length, TWriteData *data)
+{
+
+}
+
+unsigned char dyn_master_start_sync_read(TDynamixelMaster *master,unsigned char num,unsigned char *ids,unsigned short int address, unsigned short int length, TWriteData *data)
+{
+
+}
+
+unsigned char dyn_master_is_sync_read_done(TDynamixelMaster *master,unsigned char num,unsigned char *ids,unsigned short int address, unsigned short int length, TWriteData *data)
+{
+
+}
+
+unsigned char dyn_master_start_bulk_write(TDynamixelMaster *master,unsigned char num,unsigned char *ids,unsigned short int *address, unsigned short int *length, TWriteData *data)
+{
+
+}
+
+unsigned char dyn_master_is_bulk_write_done(TDynamixelMaster *master,unsigned char num,unsigned char *ids,unsigned short int *address, unsigned short int *length, TWriteData *data)
+{
+
+}
+
+unsigned char dyn_master_start_bulk_read(TDynamixelMaster *master,unsigned char num,unsigned char *ids,unsigned short int *address, unsigned short int *length, TWriteData *data)
+{
+
+}
+
+unsigned char dyn_master_is_bulk_read_done(TDynamixelMaster *master,unsigned char num,unsigned char *ids,unsigned short int *address, unsigned short int *length, TWriteData *data)
+{
+
+}
+
+// module functions
+unsigned char single_op_data[8];
+unsigned char sync_op_data[64];
+unsigned char bulk_op_data[64];
+
+void module1_init(void)
+{
+  unsigned int i;
+
+  printf("module1: init function\n");
+  for(i=0;i<8;i++)
+    single_op_data[i]=i;
+  for(i=0;i<64;i++)
+  {
+    sync_op_data[i]=i;
+    bulk_op_data[i]=i;
+  }
+}
+
+void module1_add_device(unsigned char id,unsigned short int model)
+{
+  printf("module1: add_device function\n");
+  printf("  ID: %d model: %d\n",id,model);
+}
+
+void module1_setup(void)
+{
+  printf("module1: setup function\n");
+  dyn_manager_reg_single_op(DYN_MANAGER_WRITE,11,0x0000,2,&single_op_data[0],DYN_MANAGER_PERIODIC);
+  dyn_manager_reg_single_op(DYN_MANAGER_WRITE,11,0x0000,1,&single_op_data[2],2);
+  dyn_manager_reg_single_op(DYN_MANAGER_READ,13,0x0001,2,&single_op_data[3],6);
+  dyn_manager_reg_single_op(DYN_MANAGER_READ,15,0x0005,4,&single_op_data[0],8);
+}
+
+void module1_pre_process(void)
+{
+  printf("module1: pre_process function\n");
+}
+
+void module1_post_process(void)
+{
+  printf("module1: post_process function\n");
+}
+
+void module2_init(void)
+{
+  printf("module2: init function\n");
+}
+
+void module2_add_device(unsigned char id,unsigned short int model)
+{
+  printf("module2: add_device function\n");
+  printf("  ID: %d model: %d\n",id,model);
+}
+
+void module2_setup(void)
+{
+  printf("module2: setup function\n");
+}
+
+void module2_pre_process(void)
+{
+  printf("module2: pre_process function\n");
+}
+
+void module2_post_process(void)
+{
+  printf("module2: post_process function\n");
+}
+
+int main(void)
+{
+  int i;
+  TDynModule module1;
+  TDynModule module2;
+  TTime dyn_master1_timer; 
+  TComm dyn_master1_comm;
+  TTime dyn_master2_timer; 
+  TComm dyn_master2_comm;
+
+  // init modules
+  dyn_module_init(&module1);
+  dyn_module_add_model(&module1,SERVO_MX28);
+  dyn_module_add_model(&module1,SERVO_MX28);// this should be ignored
+  dyn_module_add_model(&module1,SERVO_XL320);
+  module1.init=module1_init;
+  module1.add_device=module1_add_device;
+  module1.setup=module1_setup;
+  module1.pre_process=module1_pre_process;
+  module1.post_process=module1_post_process;
+  dyn_module_init(&module2);
+  dyn_module_add_model(&module2,SERVO_AX12A);
+  module2.init=module2_init;
+  module2.add_device=module2_add_device;
+  module2.setup=module2_setup;
+  module2.pre_process=module2_pre_process;
+  module2.post_process=module2_post_process;
+
+  // init masters
+  time_init(&dyn_master1_timer,time_get_counts_per_us(),time_get_counts);
+  comm_init(&dyn_master1_comm,0x01,&dyn_master1_timer);
+  dyn_master_init(&dyn_master1,&dyn_master1_comm,DYN_VER1);
+  time_init(&dyn_master2_timer,time_get_counts_per_us(),time_get_counts);
+  comm_init(&dyn_master2_comm,0x01,&dyn_master2_timer);
+  dyn_master_init(&dyn_master2,&dyn_master2_comm,DYN_VER2);
+
+  dyn_manager_init();
+  dyn_manager_add_master(&dyn_master1);
+  dyn_manager_add_master(&dyn_master2);
+  dyn_manager_add_module(&module1);
+  dyn_manager_add_module(&module2);
+  dyn_manager_scan();
+
+  for(i=0;i<10;i++)
+    dyn_manager_loop();
+}