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/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