diff --git a/dynamixel_base/include/dynamixel_common.h b/dynamixel_base/include/dynamixel_common.h
index 8ad6ab0f8e7fc90169f9e5e22b3a43d66bc3fde4..d8310e95182a860d7fb9ac461faa832717e9887d 100644
--- a/dynamixel_base/include/dynamixel_common.h
+++ b/dynamixel_base/include/dynamixel_common.h
@@ -14,6 +14,10 @@ typedef struct{
   unsigned char *data_addr;
 }TWriteData;
 
+typedef enum{no_return=0x00,
+             return_only_read=0x01,
+             return_all=0x02} return_level_t;
+
 // possible packet types
 typedef enum{DYN_PING=0x01,
              DYN_READ=0x02,
diff --git a/dynamixel_base/include/dynamixel_master.h b/dynamixel_base/include/dynamixel_master.h
index c375c47962624d4b41d4e4a7c2bc27c6c1e92e0b..87eb846ad79a3cea7040373196e75542711feda8 100644
--- a/dynamixel_base/include/dynamixel_master.h
+++ b/dynamixel_base/include/dynamixel_master.h
@@ -12,6 +12,7 @@ typedef struct
   TComm *comm_dev;
   unsigned char tx_buffer[MAX_TX_BUFFER_LEN];
   unsigned char rx_buffer[MAX_RX_BUFFER_LEN];
+  return_level_t return_level;
   unsigned char packet_ready;
   unsigned char received_bytes;
   void (*set_tx_mode)(void);
@@ -24,6 +25,8 @@ typedef struct
 /* public functions */
 void dyn_master_init(TDynamixelMaster *master,TComm *dev);
 void dyn_master_set_rx_timeout(TDynamixelMaster *master,unsigned short int timeout_ms);
+inline void dyn_master_set_return_level(TDynamixelMaster *master,return_level_t level);
+inline return_level_t dyn_master_get_return_level(TDynamixelMaster *master);
 void dyn_master_scan(TDynamixelMaster *master,unsigned char *num,unsigned char *ids);
 unsigned char dyn_master_ping(TDynamixelMaster *master,unsigned char id);
 unsigned char dyn_master_read_byte(TDynamixelMaster *master,unsigned char id,unsigned short int address,unsigned char *data);
diff --git a/dynamixel_base/include/dynamixel_slave.h b/dynamixel_base/include/dynamixel_slave.h
index 69572202c5cc4638f93b58e988d54f679c9630ae..34ddc8e38a795d033e12dd26f6dbbb53f08d9612 100644
--- a/dynamixel_base/include/dynamixel_slave.h
+++ b/dynamixel_base/include/dynamixel_slave.h
@@ -16,10 +16,6 @@
   #define       MAX_DYN_SLAVE_REG_BUFFER_LEN     1024
 #endif
 
-typedef enum{no_return=0x00,
-             return_only_read=0x01,
-             return_all=0x02} return_level_t;
-
 typedef struct
 {
   TComm *comm_dev;
diff --git a/dynamixel_base/src/dynamixel_master.c b/dynamixel_base/src/dynamixel_master.c
index a3d32d45741d514171adb054201ac47343da3909..7295fe5bdbe28b8e695e48233a4414aaf14c6643 100644
--- a/dynamixel_base/src/dynamixel_master.c
+++ b/dynamixel_base/src/dynamixel_master.c
@@ -90,7 +90,6 @@ void dummy_dyn_master_set_rx_mode(void)
 
 unsigned char dyn_master_wait_transmission(TDynamixelMaster *master)
 {
-  unsigned short int timeout_left=master->rx_timeout_ms*1000;
   unsigned char error;
 
   while((error=comm_is_send_done(master->comm_dev))==COMM_BUSY);
@@ -160,6 +159,7 @@ void dyn_master_init(TDynamixelMaster *master,TComm *dev)
   master->rx_timeout_ms=50;
   master->rx_no_answer=0x00;
   master->rx_num_packets=0x00;
+  master->return_level=return_all;
 
   master->set_rx_mode();
 }
@@ -169,6 +169,16 @@ void dyn_master_set_rx_timeout(TDynamixelMaster *master,unsigned short int timeo
   master->rx_timeout_ms=timeout_ms;
 }
 
+inline void dyn_master_set_return_level(TDynamixelMaster *master,return_level_t level)
+{
+  master->return_level=level;
+}
+
+inline return_level_t dyn_master_get_return_level(TDynamixelMaster *master)
+{
+  return master->return_level;
+}
+
 void dyn_master_scan(TDynamixelMaster *master,unsigned char *num,unsigned char *ids)
 {
 
@@ -204,51 +214,227 @@ unsigned char dyn_master_ping(TDynamixelMaster *master,unsigned char id)
 
 unsigned char dyn_master_read_byte(TDynamixelMaster *master,unsigned char id,unsigned short int address,unsigned char *data)
 {
-
+  return dyn_master_read_table(master,id,address,1,data);
 }
 
 unsigned char dyn_master_read_word(TDynamixelMaster *master,unsigned char id,unsigned short int address,unsigned short int *data)
 {
+  unsigned char error;
+  unsigned char data_int[2];
 
+  // generate the ping packet for the desired device
+  error=dyn_master_read_table(master,id,address,2,data_int);
+  (*data)=data_int[0]+data_int[1]*256;
+
+  return error;
 }
 
 unsigned char dyn_master_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
+  dyn_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;
+  }
+  // wait for the transmission to end
+  if((error=dyn_master_wait_transmission(master))!=DYN_SUCCESS)
+  {
+    master->set_rx_mode();
+    return error;
+  }
+  // wait for the replay within the given timeout
+  if(master->return_level!=no_return && id!=DYN_BROADCAST_ID)
+  {
+    if((error=dyn_master_wait_reception(master))==DYN_SUCCESS)
+    {
+      if(dyn_get_read_status_data(master->rx_buffer,data)!=length)// not enough data
+        error=DYN_INST_ERROR;
+    }
+  } 
 
+  return error;
 }
 
 unsigned char dyn_master_write_byte(TDynamixelMaster *master,unsigned char id, unsigned short int address, unsigned char data)
 {
-
+  return dyn_master_write_table(master,id,address,1,&data);
 }
 
 unsigned char dyn_master_write_word(TDynamixelMaster *master,unsigned char id, unsigned short int address, unsigned short int data)
 {
+  unsigned char data_int[2];
 
+  data_int[0]=data%256;
+  data_int[1]=data/256;
+  return dyn_master_write_table(master,id,address,2,data_int);
 }
 
 unsigned char dyn_master_write_table(TDynamixelMaster *master,unsigned char id, unsigned short int address, unsigned short int length, unsigned char *data)
 {
+  unsigned char error;
+
+  // generate the write packet for the desired device
+  dyn_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;
+  }
+  // wait for the transmission to end
+  if((error=dyn_master_wait_transmission(master))!=DYN_SUCCESS)
+  {
+    master->set_rx_mode();
+    return error;
+  }
+  // wait for the replay within the given timeout
+  if(master->return_level==return_all && id!=DYN_BROADCAST_ID)
+    error=dyn_master_wait_reception(master);
 
+  return error;
 }
 
 unsigned char dyn_master_reg_write(TDynamixelMaster *master,unsigned char id, unsigned short int address, unsigned short int length, unsigned char *data)
 {
+  unsigned char error;
+
+  // generate the registered write packet for the desired device
+  dyn_init_reg_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;
+  }
+  // wait for the transmission to end
+  if((error=dyn_master_wait_transmission(master))!=DYN_SUCCESS)
+  {
+    master->set_rx_mode();
+    return error;
+  }
+  // wait for the replay within the given timeout
+  if(master->return_level==return_all && id!=DYN_BROADCAST_ID)
+    error=dyn_master_wait_reception(master);
 
+  return error;
 }
 
 unsigned char dyn_master_action(TDynamixelMaster *master)
 {
+  unsigned char error;
+
+  // generate the action packet for the desired device
+  dyn_init_action_packet(master->tx_buffer);
+  master->rx_num_packets=0x01;
+  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;
+  }
+  // wait for the transmission to end
+  if((error=dyn_master_wait_transmission(master))!=DYN_SUCCESS)
+  {
+    master->set_rx_mode();
+    return error;
+  }
 
+  return error;
 }
 
 unsigned char dyn_master_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 write packet for the desired device
+  dyn_init_sync_write_packet(master->tx_buffer,num,ids,address,length,data);
+  master->rx_num_packets=0x01;
+  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;
+  }
+  // wait for the transmission to end
+  if((error=dyn_master_wait_transmission(master))!=DYN_SUCCESS)
+  {
+    master->set_rx_mode();
+    return error;
+  }
 
+  return error;
 }
 
 unsigned char dyn_master_bulk_read(TDynamixelMaster *master,unsigned char num,unsigned char *ids,unsigned short int *address, unsigned short int *length, TWriteData *data)
 {
+  unsigned char error,i;
+
+  // generate the read packet for the desired device
+  dyn_init_bulk_read_packet(master->tx_buffer,num,ids,address,length);
+  master->rx_num_packets=0x01;
+  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;
+  }
+  // wait for the transmission to end
+  if((error=dyn_master_wait_transmission(master))!=DYN_SUCCESS)
+  {
+    master->set_rx_mode();
+    return error;
+  }
+  // wait for the replay within the given timeout
+  if(master->return_level!=no_return)
+  {
+    for(i=0;i<num;i++)
+    {
+      if((error=dyn_master_wait_reception(master))==DYN_SUCCESS)
+      {
+        if(dyn_get_read_status_data(master->rx_buffer,data[i].data_addr)!=length[i])// not enough data
+          error=DYN_INST_ERROR;
+      }
+    }
+  }
 
+  return error;
 }