diff --git a/src/dynamixel.cpp b/src/dynamixel.cpp
index b5449c074987ae989e88e424ef3a1a3c71826ded..7c85ff51a3bde2ff5e0469130f6a9316360d3ce6 100644
--- a/src/dynamixel.cpp
+++ b/src/dynamixel.cpp
@@ -112,9 +112,9 @@ void CDynamixel::resync(void)
 void CDynamixel::read_byte_register(unsigned char address,unsigned char *value)
 {
   unsigned char packet[8]={0xFF,0xFF,0x00,0x04,0x02,0x00,0x00,0x00};
-  int num=0,read=0,it=0,read_length=7;
+  int num=0,read=0,it=0,read_length=7,i=0;
   std::list<std::string> events;
-  unsigned char data[7];
+  unsigned char data[7],dummy;
   std::string error_msg;
   bool rx_tx_ok=false;
 
@@ -142,7 +142,14 @@ void CDynamixel::read_byte_register(unsigned char address,unsigned char *value)
           {
             this->event_server->wait_all(events,100);
             num=this->usb_dev->get_num_data();
-            this->usb_dev->read(&data[read],num);
+            if(read+num>read_length)
+            {
+              this->usb_dev->read(&data[read],read_length-read);
+              for(i=0;i<(num-read_length+read);i++)
+                this->usb_dev->read(&dummy,1);
+            }
+            else
+              this->usb_dev->read(&data[read],num);  
             read+=num;
             if(read>5 && data[4]!=0x00)
               read_length=6;
@@ -203,9 +210,9 @@ void CDynamixel::read_byte_register(unsigned char address,unsigned char *value)
 void CDynamixel::read_word_register(unsigned char address,unsigned short int *value)
 {
   unsigned char packet[8]={0xFF,0xFF,0x00,0x04,0x02,0x00,0x00,0x00};
-  int num=0,read=0,it=0,read_length=8;
+  int num=0,read=0,it=0,read_length=8,i=0;
   std::list<std::string> events;
-  unsigned char data[8];
+  unsigned char data[8],dummy;
   std::string error_msg;
   bool rx_tx_ok=false;
 
@@ -233,7 +240,14 @@ void CDynamixel::read_word_register(unsigned char address,unsigned short int *va
           {
             this->event_server->wait_all(events,100);
             num=this->usb_dev->get_num_data();
-            this->usb_dev->read(&data[read],num);
+            if(read+num>read_length)
+            {
+              this->usb_dev->read(&data[read],read_length-read);
+              for(i=0;i<(num-read_length+read);i++)
+                this->usb_dev->read(&dummy,1);
+            }
+            else
+              this->usb_dev->read(&data[read],num);  
             read+=num;
             if(read>5 && data[4]!=0x00)
               read_length=6;
@@ -295,9 +309,9 @@ void CDynamixel::write_byte_register(unsigned char address, unsigned char data)
 {
   unsigned char packet[8]={0xFF,0xFF,0x00,0x04,0x03,0x00,0x00,0x00};
   std::list<std::string> events;
-  unsigned char answer[6];
+  unsigned char answer[6],dummy;
+  int num=0,read=0,it=0,i=0;
   std::string error_msg;
-  int num=0,read=0,it=0;
   bool rx_tx_ok=false;
 
   if(this->usb_dev!=NULL)
@@ -324,7 +338,14 @@ void CDynamixel::write_byte_register(unsigned char address, unsigned char data)
           {
             this->event_server->wait_all(events,100);
             num=this->usb_dev->get_num_data();
-            this->usb_dev->read(&answer[read],num);
+            if(read+num>6)
+            {
+              this->usb_dev->read(&answer[read],6-read);
+              for(i=0;i<(num-6+read);i++)
+                this->usb_dev->read(&dummy,1);
+            }
+            else
+              this->usb_dev->read(&answer[read],num);  
             read+=num;
           }
           if(this->compute_checksum(answer,6)!=0)
@@ -379,9 +400,9 @@ void CDynamixel::write_word_register(unsigned char address, unsigned short int d
 {
   unsigned char packet[9]={0xFF,0xFF,0x00,0x05,0x03,0x00,0x00,0x00,0x00};
   std::list<std::string> events;
-  unsigned char answer[6];
+  unsigned char answer[6],dummy;
+  int num=0,read=0,it=0,i=0;
   std::string error_msg;
-  int num=0,read=0,it=0;
   bool rx_tx_ok=false;
 
   if(this->usb_dev!=NULL)
@@ -409,7 +430,14 @@ void CDynamixel::write_word_register(unsigned char address, unsigned short int d
           {
             this->event_server->wait_all(events,100);
             num=this->usb_dev->get_num_data();
-            this->usb_dev->read(&answer[read],num);
+            if(read+num>6)
+            {
+              this->usb_dev->read(&answer[read],6-read);
+              for(i=0;i<(num-6+read);i++)
+                this->usb_dev->read(&dummy,1);
+            }
+            else
+              this->usb_dev->read(&answer[read],num);  
             read+=num;
           }
           if(this->compute_checksum(answer,6)!=0)
@@ -464,9 +492,9 @@ void CDynamixel::registered_byte_write(unsigned char address, unsigned char data
 {
   unsigned char packet[8]={0xFF,0xFF,0x00,0x04,0x04,0x00,0x00,0x00};
   std::list<std::string> events;
-  unsigned char answer[6];
+  unsigned char answer[6],dummy;
+  int num=0,read=0,it=0,i=0;
   std::string error_msg;
-  int num=0,read=0,it=0;
   bool rx_tx_ok=false;
 
   if(this->usb_dev!=NULL)
@@ -493,7 +521,14 @@ void CDynamixel::registered_byte_write(unsigned char address, unsigned char data
           {
             this->event_server->wait_all(events,100);
             num=this->usb_dev->get_num_data();
-            this->usb_dev->read(&answer[read],num);
+            if(read+num>6)
+            {
+              this->usb_dev->read(&answer[read],6-read);
+              for(i=0;i<(num-6+read);i++)
+                this->usb_dev->read(&dummy,1);
+            }
+            else
+              this->usb_dev->read(&answer[read],num);  
             read+=num;
           }
           if(this->compute_checksum(answer,6)!=0)
@@ -548,9 +583,9 @@ void CDynamixel::registered_word_write(unsigned char address, unsigned short int
 {
   unsigned char packet[9]={0xFF,0xFF,0x00,0x05,0x04,0x00,0x00,0x00,0x00};
   std::list<std::string> events;
-  unsigned char answer[6];
+  unsigned char answer[6],dummy;
+  int num=0,read=0,it=0,i=0;
   std::string error_msg;
-  int num=0,read=0,it=0;
   bool rx_tx_ok=false;
 
   if(this->usb_dev!=NULL)
@@ -578,7 +613,14 @@ void CDynamixel::registered_word_write(unsigned char address, unsigned short int
           {
             this->event_server->wait_all(events,100);
             num=this->usb_dev->get_num_data();
-            this->usb_dev->read(&answer[read],num);
+            if(read+num>6)
+            {
+              this->usb_dev->read(&answer[read],6-read);
+              for(i=0;i<(num-6+read);i++)
+                this->usb_dev->read(&dummy,1);
+            }
+            else
+              this->usb_dev->read(&answer[read],num);  
             read+=num;
           }
           if(this->compute_checksum(answer,6)!=0)
@@ -633,9 +675,9 @@ void CDynamixel::write_registers(unsigned char address, unsigned char *data, uns
 {
   unsigned char *packet;
   std::list<std::string> events;
-  unsigned char answer[6];
-  std::string error_msg;
+  unsigned char answer[6],dummy;
   int num=0,read=0,it=0;
+  std::string error_msg;
   bool rx_tx_ok=false;
   unsigned int i=0;
 
@@ -678,7 +720,14 @@ void CDynamixel::write_registers(unsigned char address, unsigned char *data, uns
             {
               this->event_server->wait_all(events,100);
               num=this->usb_dev->get_num_data();
-              this->usb_dev->read(&answer[read],num);
+              if(read+num>6)
+              {
+                this->usb_dev->read(&answer[read],6-read);
+                for(i=0;i<(num-6+read);i++)
+                  this->usb_dev->read(&dummy,1);
+              }
+              else
+                this->usb_dev->read(&answer[read],num);  
               read+=num;
             }
             if(this->compute_checksum(answer,6)!=0)
@@ -735,7 +784,7 @@ void CDynamixel::read_registers(unsigned char address, unsigned char *data, unsi
   unsigned char packet[8]={0xFF,0xFF,0x00,0x04,0x02,0x00,0x00,0x00};
   int num=0,read=0,it=0,read_length=length+6;
   std::list<std::string> events;
-  unsigned char *answer;
+  unsigned char *answer,dummy;
   std::string error_msg;
   bool rx_tx_ok=false;
   unsigned int i=0;
@@ -765,7 +814,14 @@ void CDynamixel::read_registers(unsigned char address, unsigned char *data, unsi
           {
             this->event_server->wait_all(events,100);
             num=this->usb_dev->get_num_data();
-            this->usb_dev->read(&answer[read],num);
+            if(read+num>read_length)
+            {
+              this->usb_dev->read(&answer[read],read_length-read);
+              for(i=0;i<(num-read_length+read);i++)
+                this->usb_dev->read(&dummy,1);
+            }
+            else
+              this->usb_dev->read(&answer[read],num);  
             read+=num;
             if(read>5 && answer[4]!=0x00)
               read_length=6;