diff --git a/src/dynamixel.cpp b/src/dynamixel.cpp
index 029b0ed755714683aa979eb8cc269bf153286f11..6367c5551aa26e70330f1774fe48c4beb06824ee 100644
--- a/src/dynamixel.cpp
+++ b/src/dynamixel.cpp
@@ -107,7 +107,7 @@ unsigned char CDynamixel::receive_status_packet_v1(unsigned char **data,unsigned
 {
   std::list<std::string> events;
   unsigned char data_int[1024];
-  int num=0,read=0,length,start=0;
+  int num=0,read=0,length=-1,start=0;
 
   if(this->comm_dev!=NULL)
   {
@@ -418,7 +418,7 @@ void CDynamixel::sync_packet_v2(unsigned char *data,unsigned int length,int *sta
 
 void CDynamixel::read_byte_register(unsigned short int address,unsigned char *value)
 {
-  unsigned char *data,error,length_v1,cmd[4];
+  unsigned char *data=NULL,error,length_v1,cmd[4];
   unsigned short length_v2;
 
   if(address!=(unsigned short int)-1)
@@ -442,36 +442,34 @@ void CDynamixel::read_byte_register(unsigned short int address,unsigned char *va
         this->send_instruction_packet_v2(dyn_read,cmd,4);
         error=this->receive_status_packet_v2(&data,&length_v2);
       }
+      this->handle_error(error);
+      if (data != NULL) 
+      {
+        *value=data[0];
+        delete[] data;
+      }
       this->usb_access->exit();
     }
     catch(CEventTimeoutException &e)
     {
+      if (data != NULL) 
+        delete[] data;
       this->usb_access->exit();
       throw e;
     }
     catch(CException &e)
     {
-      this->usb_access->exit();
-      throw e;
-    }
-    try{
-      this->handle_error(error);
       if (data != NULL) 
-      {
-        *value=data[0];
-        delete[] data;
-      }
-    }catch(CException &e){
-      if(data!=NULL)
         delete[] data;
+      this->usb_access->exit();
       throw e;
-    } 
+    }
   }
 }
 
 void CDynamixel::read_word_register(unsigned short int address,unsigned short int *value)
 {
-  unsigned char *data,error,length_v1,cmd[4];
+  unsigned char *data=NULL,error,length_v1,cmd[4];
   unsigned short length_v2;
 
   if(address!=(unsigned short int)-1)
@@ -495,28 +493,26 @@ void CDynamixel::read_word_register(unsigned short int address,unsigned short in
         this->send_instruction_packet_v2(dyn_read,cmd,4);
         error=this->receive_status_packet_v2(&data,&length_v2);
       }
+      this->handle_error(error);
+      if (data != NULL) 
+      {
+        *value=data[0]+data[1]*256;
+        delete[] data;
+      }
       this->usb_access->exit();
     }
     catch(CEventTimeoutException &e)
     {
+      if(data!=NULL)
+        delete[] data;
       this->usb_access->exit();
       throw e;
     }
     catch(CException &e)
     {
-      this->usb_access->exit();
-      throw e;
-    }
-    try{
-      this->handle_error(error);
-      if (data != NULL) 
-      {
-        *value=data[0]+data[1]*256;
-        delete[] data;
-      }
-    }catch(CException &e){
       if(data!=NULL)
         delete[] data;
+      this->usb_access->exit();
       throw e;
     }
   }
@@ -524,7 +520,7 @@ void CDynamixel::read_word_register(unsigned short int address,unsigned short in
 
 void CDynamixel::write_byte_register(unsigned short int address, unsigned char value)
 {
-  unsigned char *data,error,length_v1,cmd[3];
+  unsigned char *data=NULL,error,length_v1,cmd[3];
   unsigned short length_v2;
 
   if(address!=(unsigned short int)-1)
@@ -554,11 +550,15 @@ void CDynamixel::write_byte_register(unsigned short int address, unsigned char v
     }
     catch(CEventTimeoutException &e)
     {
+      if(data!=NULL)
+        delete[] data;
       this->usb_access->exit();
       throw e;
     }
     catch(CException &e)
     {
+      if(data!=NULL)
+        delete[] data;
       this->usb_access->exit();
       throw e;
     }
@@ -567,7 +567,7 @@ void CDynamixel::write_byte_register(unsigned short int address, unsigned char v
 
 void CDynamixel::write_word_register(unsigned short int address, unsigned short int value)
 {
-  unsigned char *data,error,length_v1,cmd[4];
+  unsigned char *data=NULL,error,length_v1,cmd[4];
   unsigned short length_v2;
 
   if(address!=(unsigned short int)-1)
@@ -599,11 +599,15 @@ void CDynamixel::write_word_register(unsigned short int address, unsigned short
     }
     catch(CEventTimeoutException &e)
     {
+      if(data!=NULL)
+        delete[] data;
       this->usb_access->exit();
       throw e;
     }
     catch(CException &e)
     {
+      if(data!=NULL)
+        delete[] data;
       this->usb_access->exit();
       throw e;
     }
@@ -612,7 +616,7 @@ void CDynamixel::write_word_register(unsigned short int address, unsigned short
 
 void CDynamixel::registered_byte_write(unsigned short int address, unsigned char value)
 {
-  unsigned char *data,error,length_v1,cmd[3];
+  unsigned char *data=NULL,error,length_v1,cmd[3];
   unsigned short length_v2;
 
   if(address!=(unsigned short int)-1)
@@ -642,11 +646,15 @@ void CDynamixel::registered_byte_write(unsigned short int address, unsigned char
     }
     catch(CEventTimeoutException &e)
     {
+      if(data!=NULL)
+        delete[] data;
       this->usb_access->exit();
       throw e;
     }
     catch(CException &e)
     {
+      if(data!=NULL)
+        delete[] data;
       this->usb_access->exit();
       throw e;
     }
@@ -655,7 +663,7 @@ void CDynamixel::registered_byte_write(unsigned short int address, unsigned char
 
 void CDynamixel::registered_word_write(unsigned short int address, unsigned short int value)
 {
-  unsigned char *data,error,length_v1,cmd[4];
+  unsigned char *data=NULL,error,length_v1,cmd[4];
   unsigned short length_v2;
 
   if(address==(unsigned short int)-1)
@@ -687,11 +695,15 @@ void CDynamixel::registered_word_write(unsigned short int address, unsigned shor
     }
     catch(CEventTimeoutException &e)
     {
+      if(data!=NULL)
+        delete[] data;
       this->usb_access->exit();
       throw e;
     }
     catch(CException &e)
     {
+      if(data!=NULL)
+        delete[] data;
       this->usb_access->exit();
       throw e;
     }
@@ -700,7 +712,7 @@ void CDynamixel::registered_word_write(unsigned short int address, unsigned shor
 
 void CDynamixel::registered_write(unsigned short int address, unsigned char *values,unsigned int length)
 {
-  unsigned char *data,error,length_v1,*cmd;
+  unsigned char *data=NULL,error,length_v1,*cmd;
   unsigned short length_v2,i;
 
   if(address!=(unsigned short int)-1)
@@ -736,11 +748,15 @@ void CDynamixel::registered_write(unsigned short int address, unsigned char *val
     }
     catch(CEventTimeoutException &e)
     {
+      if(data!=NULL)
+        delete[] data;
       this->usb_access->exit();
       throw e;
     }
     catch(CException &e)
     {
+      if(data!=NULL)
+        delete[] data;
       this->usb_access->exit();
       throw e;
     }
@@ -749,7 +765,7 @@ void CDynamixel::registered_write(unsigned short int address, unsigned char *val
 
 void CDynamixel::write_registers(unsigned short int address, unsigned char *values, unsigned int length)
 {
-  unsigned char *data,error,length_v1,*cmd;
+  unsigned char *data=NULL,error,length_v1,*cmd;
   unsigned short length_v2,i;
 
   if(address!=(unsigned short int)-1)
@@ -785,11 +801,15 @@ void CDynamixel::write_registers(unsigned short int address, unsigned char *valu
     }
     catch(CEventTimeoutException &e)
     {
+      if(data!=NULL)
+        delete[] data;
       this->usb_access->exit();
       throw e;
     }
     catch(CException &e)
     {
+      if(data!=NULL)
+        delete[] data;
       this->usb_access->exit();
       throw e;
     }
@@ -798,7 +818,7 @@ void CDynamixel::write_registers(unsigned short int address, unsigned char *valu
 
 void CDynamixel::read_registers(unsigned short int address, unsigned char *values, unsigned int length)
 {
-  unsigned char *data,error,length_v1,cmd[4];
+  unsigned char *data=NULL,error,length_v1,cmd[4];
   unsigned short length_v2,i;
 
   if(address!=(unsigned short int)-1)
@@ -822,29 +842,29 @@ void CDynamixel::read_registers(unsigned short int address, unsigned char *value
         this->send_instruction_packet_v2(dyn_read,cmd,4);
         error=this->receive_status_packet_v2(&data,&length_v2);
       }
+      this->handle_error(error);
+      if(data!=NULL) 
+      {
+        for(i=0;i<length;i++)
+          values[i]=data[i];
+        delete[] data;
+      }
       this->usb_access->exit();
     }
     catch(CEventTimeoutException &e)
     {
+      std::cout << "Exception timeout read registers" << std::endl;
+      if(data!=NULL)
+        delete[] data;
       this->usb_access->exit();
       throw e;
     }
     catch(CException &e)
     {
-      this->usb_access->exit();
-      throw e;
-    }
-    try{
-      this->handle_error(error);
-      if(data!=NULL) 
-      {
-        for(i=0;i<length;i++)
-          values[i]=data[i];
-        delete[] data;
-      }
-    }catch(CException &e){
+      std::cout << "Exception error read registers" << std::endl;
       if(data!=NULL)
         delete[] data;
+      this->usb_access->exit();
       throw e;
     }
   }
@@ -852,7 +872,7 @@ void CDynamixel::read_registers(unsigned short int address, unsigned char *value
 
 void CDynamixel::reset(dyn_reset_mode_t mode)
 {
-  unsigned char *data,error,length_v1,cmd;
+  unsigned char *data=NULL,error,length_v1,cmd;
   unsigned short length_v2;
 
   try
@@ -869,25 +889,23 @@ void CDynamixel::reset(dyn_reset_mode_t mode)
       this->send_instruction_packet_v2(dyn_read,&cmd,1);
       error=this->receive_status_packet_v2(&data,&length_v2);
     }
+    this->handle_error(error);
+    if(data!=NULL)
+      delete[] data;
     this->usb_access->exit();
   }
   catch(CEventTimeoutException &e)
   {
+    if(data!=NULL)
+      delete[] data;
     this->usb_access->exit();
     throw e;
   }
   catch(CException &e)
   {
-    this->usb_access->exit();
-    throw e;
-  }
-  try{
-    this->handle_error(error);
-    if(data!=NULL)
-      delete[] data;
-  }catch(CException &e){
     if(data!=NULL)
       delete[] data;
+    this->usb_access->exit();
     throw e;
   }
 }