diff --git a/communications/src/dynamixel_master.c b/communications/src/dynamixel_master.c
index a845cba475032de6e19183814a80f2b249834fa6..08b286db9711db97eb84de81445848cba36041cb 100644
--- a/communications/src/dynamixel_master.c
+++ b/communications/src/dynamixel_master.c
@@ -61,6 +61,7 @@ void dyn_master_set_rx_mode(void)
 /* interrupt handlers */
 ISR(USART0_TX_vect)
 {
+  cli();
   if(dyn_master_sent_bytes==dyn_master_tx_buffer[3]+4)
   {
     dyn_master_set_rx_mode();
@@ -71,6 +72,7 @@ ISR(USART0_TX_vect)
     UDR0=dyn_master_tx_buffer[dyn_master_sent_bytes];
     dyn_master_sent_bytes++;
   }
+  sei();
 }
 
 ISR(USART0_RX_vect)
@@ -127,7 +129,6 @@ uint8_t dyn_master_send(void)
 uint8_t dyn_master_wait_reception(void)
 {
   // set timeout 
-  dyn_master_start_timeout();
   // wait for the status packet
   if(dyn_master_rx_no_answer)
   {
@@ -136,6 +137,7 @@ uint8_t dyn_master_wait_reception(void)
   }
   else
   {
+    dyn_master_start_timeout();
     while(dyn_master_packet_ready==0x00)
     {
       if(dyn_master_timeout())
@@ -278,12 +280,6 @@ uint8_t dyn_master_ping(uint8_t id)
     dyn_master_set_rx_mode();
     return error;
   }
-  // wait for the transmission to end
-  if((error=dyn_master_wait_transmission())!=DYN_SUCCESS)
-  {
-    dyn_master_set_rx_mode();
-    return error;
-  }
   // wait for the replay within the given timeout
   error=dyn_master_wait_reception();
 
@@ -328,12 +324,6 @@ uint8_t dyn_master_read_table(uint8_t id,uint16_t address,uint16_t length,uint8_
     dyn_master_set_rx_mode();
     return error;
   }
-  // wait for the transmission to end
-  if((error=dyn_master_wait_transmission())!=DYN_SUCCESS)
-  {
-    dyn_master_set_rx_mode();
-    return error;
-  }
   // wait for the replay within the given timeout
   if(dyn_master_return_level!=no_return && id!=DYN_BROADCAST_ID)
   {
@@ -382,12 +372,6 @@ uint8_t dyn_master_write_table(uint8_t id, uint16_t address, uint16_t length, ui
     dyn_master_set_rx_mode();
     return error;
   }
-  // wait for the transmission to end
-  if((error=dyn_master_wait_transmission())!=DYN_SUCCESS)
-  {
-    dyn_master_set_rx_mode();
-    return error;
-  }
   // wait for the replay within the given timeout
   if(dyn_master_return_level==return_all && id!=DYN_BROADCAST_ID)
     error=dyn_master_wait_reception();
@@ -416,12 +400,6 @@ uint8_t dyn_master_reg_write(uint8_t id, uint16_t address, uint16_t length, uint
     dyn_master_set_rx_mode();
     return error;
   }
-  // wait for the transmission to end
-  if((error=dyn_master_wait_transmission())!=DYN_SUCCESS)
-  {
-    dyn_master_set_rx_mode();
-    return error;
-  }
   // wait for the replay within the given timeout
   if(dyn_master_return_level==return_all && id!=DYN_BROADCAST_ID)
     error=dyn_master_wait_reception();
@@ -447,12 +425,6 @@ uint8_t dyn_master_action(void)
     dyn_master_set_rx_mode();
     return error;
   }
-  // wait for the transmission to end
-  if((error=dyn_master_wait_transmission())!=DYN_SUCCESS)
-  {
-    dyn_master_set_rx_mode();
-    return error;
-  }
 
   return error;
 }
@@ -475,12 +447,6 @@ uint8_t dyn_master_sync_write(uint8_t num,uint8_t *ids,uint16_t address, uint16_
     dyn_master_set_rx_mode();
     return error;
   }
-  // wait for the transmission to end
-  if((error=dyn_master_wait_transmission())!=DYN_SUCCESS)
-  {
-    dyn_master_set_rx_mode();
-    return error;
-  }
 
   return error;
 }
@@ -511,12 +477,6 @@ uint8_t dyn_master_bulk_read(uint8_t num,uint8_t *ids,uint16_t *address, uint16_
     dyn_master_set_rx_mode();
     return error;
   }
-  // wait for the transmission to end
-  if((error=dyn_master_wait_transmission())!=DYN_SUCCESS)
-  {
-    dyn_master_set_rx_mode();
-    return error;
-  }
   // wait for the replay within the given timeout
   if(dyn_master_return_level!=no_return)
   {
diff --git a/communications/src/serial_console.c b/communications/src/serial_console.c
index 7842eb7d91bf8866bd014625a7a30f234339bbfd..64fa7f4ecc58828bdbcadd8d31eb4787ac52d085 100644
--- a/communications/src/serial_console.c
+++ b/communications/src/serial_console.c
@@ -3,21 +3,40 @@
 #include <stdio.h>
 
 /* private variables */
-volatile uint8_t serial_console_buffer[SERIAL_CONSOLE_MAX_BUFFER_LEN];
-volatile uint8_t serial_console_read_ptr;
-volatile uint8_t serial_console_write_ptr;
-volatile uint8_t serial_console_num_data;
+volatile uint8_t serial_console_rx_buffer[SERIAL_CONSOLE_MAX_BUFFER_LEN];
+volatile uint8_t serial_console_rx_read_ptr;
+volatile uint8_t serial_console_rx_write_ptr;
+volatile uint8_t serial_console_rx_num_data;
+volatile uint8_t serial_console_tx_buffer[SERIAL_CONSOLE_MAX_BUFFER_LEN];
+volatile uint8_t serial_console_tx_read_ptr;
+volatile uint8_t serial_console_tx_write_ptr;
+volatile uint8_t serial_console_tx_num_data;
+volatile uint8_t serial_console_sending;
 static FILE *device;
 
 /* interrupt handlers */
 SIGNAL(USART1_RX_vect)
 {
-  if(serial_console_num_data<SERIAL_CONSOLE_MAX_BUFFER_LEN)
+  cli();
+  serial_console_rx_buffer[serial_console_rx_write_ptr]=UDR1;
+  serial_console_rx_write_ptr=(serial_console_rx_write_ptr+1)%SERIAL_CONSOLE_MAX_BUFFER_LEN;
+  if(serial_console_rx_num_data<SERIAL_CONSOLE_MAX_BUFFER_LEN)
+    serial_console_rx_num_data++;
+  sei();
+}
+
+SIGNAL(USART1_TX_vect)
+{
+  cli();
+  if(serial_console_tx_num_data>0)
   {
-    serial_console_buffer[serial_console_write_ptr]=UDR1;
-    serial_console_write_ptr=(serial_console_write_ptr+1)%SERIAL_CONSOLE_MAX_BUFFER_LEN;
-    serial_console_num_data++;
+    UDR1=serial_console_tx_buffer[serial_console_tx_read_ptr];
+    serial_console_tx_read_ptr=(serial_console_tx_read_ptr+1)%SERIAL_CONSOLE_MAX_BUFFER_LEN;
+    serial_console_tx_num_data--;
   }
+  else
+    serial_console_sending=0x00;
+  sei();
 }
 
 /* private functions */
@@ -32,14 +51,25 @@ void serial_console_set_baudrate(uint32_t baudrate)
 
 int serial_console_putchar(char c,FILE *dev)
 {
-  if( c == '\n' )
+
+  cli();
+  if(c=='\n')
   {
-    while((UCSR1A&(1<<UDRE1))==0x00);
-    UDR1 = '\r';
+    serial_console_tx_buffer[serial_console_tx_write_ptr]='\r';
+    serial_console_tx_write_ptr=(serial_console_tx_write_ptr+1)%SERIAL_CONSOLE_MAX_BUFFER_LEN;
+    serial_console_tx_num_data++;
   }
-
-  while((UCSR1A&(1<<UDRE1))==0x00);
-  UDR1 = c;
+  serial_console_tx_buffer[serial_console_tx_write_ptr]=c;
+  serial_console_tx_write_ptr=(serial_console_tx_write_ptr+1)%SERIAL_CONSOLE_MAX_BUFFER_LEN;
+  serial_console_tx_num_data++;
+  if(serial_console_sending==0x00)
+  {
+    UDR1=serial_console_tx_buffer[serial_console_tx_read_ptr];
+    serial_console_tx_read_ptr=(serial_console_tx_read_ptr+1)%SERIAL_CONSOLE_MAX_BUFFER_LEN;
+    serial_console_tx_num_data--;
+    serial_console_sending=0x01;
+  }
+  sei();
 
   return 0;
 }
@@ -48,14 +78,22 @@ int serial_console_getchar(FILE *dev)
 {
   int8_t rx;
 
-  while(serial_console_num_data==0);
-  rx=serial_console_buffer[serial_console_read_ptr];
-  serial_console_read_ptr=(serial_console_read_ptr+1)%SERIAL_CONSOLE_MAX_BUFFER_LEN;
-  serial_console_num_data--;
-  if(rx=='\r')
-    rx='\n';
-
-  return rx;
+  cli();
+  if(serial_console_rx_num_data>0)
+  {
+    rx=serial_console_rx_buffer[serial_console_rx_read_ptr];
+    serial_console_rx_read_ptr=(serial_console_rx_read_ptr+1)%SERIAL_CONSOLE_MAX_BUFFER_LEN;
+    serial_console_rx_num_data--;
+    if(rx=='\r')
+      rx='\n';
+    sei();
+    return rx;
+  }
+  else
+  {
+    sei();
+    return -1;
+  }
 }
 
 /* pubic functions */
@@ -78,7 +116,7 @@ void serial_console_init(uint32_t baudrate)
   // bit4: enable rx
   // bit3: enable tx
   // bit2: set sendding size(0 = 8bit)
-  UCSR1B = 0b10011000;
+  UCSR1B = 0b11011000;
 
   // set UART register C
   // bit6: communication mode (1 = synchronize, 0 = asynchronize)
@@ -88,10 +126,13 @@ void serial_console_init(uint32_t baudrate)
   UCSR1C = 0b00000110;
 
   // initialize
-  UDR1 = 0xFF;
-  serial_console_read_ptr=0;
-  serial_console_write_ptr=0;
-  serial_console_num_data=0;
+  serial_console_rx_read_ptr=0;
+  serial_console_rx_write_ptr=0;
+  serial_console_rx_num_data=0;
+  serial_console_tx_read_ptr=0;
+  serial_console_tx_write_ptr=0;
+  serial_console_tx_num_data=0;
+  serial_console_sending=0x00;
 
   // set baudrate
   serial_console_set_baudrate(baudrate);
diff --git a/controllers/include/cm510_cfg.h b/controllers/include/cm510_cfg.h
index f8430c0d1d0563db04f33c99b3ba3340b2dc38d3..0b2b730ac12bbc37fba3455238aed9fe85a1d636 100644
--- a/controllers/include/cm510_cfg.h
+++ b/controllers/include/cm510_cfg.h
@@ -16,7 +16,7 @@
 #define DYN_MASTER_MAX_TX_BUFFER_LEN            128
 #define DYN_MASTER_MAX_RX_BUFFER_LEN            128
 #define DYN_MASTER_DEFAULT_BAUDRATE             1000000
-#define DYN_MASTER_DEFAULT_TIMEOUT_US           10000
+#define DYN_MASTER_DEFAULT_TIMEOUT_US           1000
 #define SERIAL_CONSOLE_MAX_BUFFER_LEN           128
 
 // motion configuration parameters