diff --git a/src/dynamixel.cpp b/src/dynamixel.cpp
index 7ddd9471fcd0fa78e48fc103d82ffac8e85aba88..61df2a885df3aec397a17fc06206a2be73be2f39 100644
--- a/src/dynamixel.cpp
+++ b/src/dynamixel.cpp
@@ -290,7 +290,6 @@ void CDynamixel::handle_error(unsigned char error)
       error_msg+="\nOverload";
     if(error&0x40)
       error_msg+="\nInvalid instruction";
-    std::cout << error_msg << std::endl;
     throw CDynamixelAlarmException(_HERE_,error_msg,this->node_address,error);
   }
 }
diff --git a/src/dynamixelserver.cpp b/src/dynamixelserver.cpp
index 822a9c06b33e73788476d795a7b02d5e71341fca..c48e6a8e44da052be9d987322ee1f4589f7b14d4 100644
--- a/src/dynamixelserver.cpp
+++ b/src/dynamixelserver.cpp
@@ -194,7 +194,9 @@ unsigned char CDynamixelServer::receive_status_packet_v1(unsigned char **data,un
       do{
         if((num=this->comm_dev->get_num_data())==0)
         {
+          this->dynamixel_access.exit();
           this->event_server->wait_all(events,20);
+          this->dynamixel_access.enter();
           num=this->comm_dev->get_num_data();
         }
         if((read+num)>1024)
@@ -215,7 +217,9 @@ unsigned char CDynamixelServer::receive_status_packet_v1(unsigned char **data,un
       {
         if((num=this->comm_dev->get_num_data())==0)
         {
+          this->dynamixel_access.exit();
           this->event_server->wait_all(events,20);
+          this->dynamixel_access.enter();
           num=this->comm_dev->get_num_data();
         }
         if((read-start+num)>length)
@@ -277,7 +281,9 @@ unsigned char CDynamixelServer::receive_status_packet_v2(unsigned char **data,un
       do{
         if((num=this->comm_dev->get_num_data())==0)
         {
+          this->dynamixel_access.exit();
           this->event_server->wait_all(events,20);
+          this->dynamixel_access.enter();
           num=this->comm_dev->get_num_data();
         }
         if((read+num)>1024)
@@ -291,14 +297,16 @@ unsigned char CDynamixelServer::receive_status_packet_v2(unsigned char **data,un
           read+=num;
         }
         this->sync_packet_v2(data_int,read,&start);
-      }while(read<7);
+      }while((read-start)<7);
       length=data_int[start+5]+data_int[start+6]*256+7;
       // read the remaining of the packet
       while((read-start)<length)
       {
         if((num=this->comm_dev->get_num_data())==0)
         {
+          this->dynamixel_access.exit();
           this->event_server->wait_all(events,20);
+          this->dynamixel_access.enter();
           num=this->comm_dev->get_num_data();
         }
         if((read-start+num)>length)