diff --git a/src/dynamixel_slave.cpp b/src/dynamixel_slave.cpp
index 8b076dcbc2d16b0c2e126760efa10719ab0c58be..41f2a4e51585bc617e8aa215e5a30a42851000bf 100644
--- a/src/dynamixel_slave.cpp
+++ b/src/dynamixel_slave.cpp
@@ -31,11 +31,11 @@ void *CDynamixelSlave::process_packets_thread(void *params)
   CDynamixelSlave *slave=(CDynamixelSlave *)params;
   std::list<std::string> events;
   int event_index,num,i;
-  unsigned char data[256],new_data[256];
+  unsigned char data[64*1024],new_data[64*1024];
   bool end=false;
 
   static bool data_phase=false;
-  static unsigned char packet[256];
+  static unsigned char packet[64*1024];
   static int num_bytes=0,length=0; 
 
   // wait until the comm device becomes valid 
@@ -59,6 +59,7 @@ void *CDynamixelSlave::process_packets_thread(void *params)
       // process the incomming data
       slave->comm_access.enter();
       num=slave->comm_dev->get_num_data();
+      std::cout << "Num data: " << num << std::endl;
       if(slave->comm_dev->read(data,num)!=num)
       {
         slave->comm_access.exit();
@@ -69,6 +70,7 @@ void *CDynamixelSlave::process_packets_thread(void *params)
         slave->comm_access.exit();
         for(i=0;i<num;i++)
 	{
+          std::cout << "num bytes: " << num_bytes << std::endl;
           if(!data_phase)
           {
             switch(num_bytes)
@@ -132,6 +134,7 @@ void *CDynamixelSlave::process_packets_thread(void *params)
           }
           else// data phase
           {
+            std::cout << "data phase: " << length << std::endl;
             packet[num_bytes]=data[i];
             num_bytes++;
             length--;
@@ -210,7 +213,7 @@ void CDynamixelSlave::dynamixel_loop_v1(void)
 {
   unsigned short int length,address;
   unsigned char error,id,prev_id;;
-  unsigned char *data,*write_data,read_data[256];
+  unsigned char *data,*write_data,read_data[64*1024];
   unsigned int i=0;
 
   data=this->packets[0];
@@ -327,7 +330,7 @@ void CDynamixelSlave::dynamixel_loop_v2(void)
 {
   unsigned char error,prev_id,id;
   unsigned short int length,address,checksum,checksum_pkt;
-  unsigned char *data,*write_data,read_data[256];
+  unsigned char *data,*write_data,read_data[64*1024];
   unsigned int i=0;
 
   data=this->packets[0];
diff --git a/src/examples/test_dynamixel_slave.cpp b/src/examples/test_dynamixel_slave.cpp
index 9b719ce7dedbee74aa8133478b33ce6829c0de10..001038c80956f34560e95f76d5a48c93531908c0 100644
--- a/src/examples/test_dynamixel_slave.cpp
+++ b/src/examples/test_dynamixel_slave.cpp
@@ -10,7 +10,7 @@ unsigned char rx28_servo_data[SERVO_MEMORY_SIZE]={0x1c,0x00,0x00,0x01,0x22,0xFA,
                                                   0x02,0x24,0x24,0x00,0x00,0x00,0x00,0x00,
                                                   0x00,0x00,0x01,0x01,0x20,0x20,0x00,0x00,
                                                   0x00,0x00,0xFF,0x03,0x00,0x00,0x00,0x00,
-                                                  0x00,0x00,0x78,0x19,0x00,0x00,0x00,0x00,
+                                                  0x00,0x00,0x78,0x29,0x00,0x00,0x00,0x00,
                                                   0x20,0x00};
 
 std::string serial="AI027ZFC";
@@ -23,17 +23,22 @@ void on_ping_03(void)
 unsigned char on_read_03(unsigned short int address, unsigned short int length, unsigned char *data)
 {
   std::cout << "servo 3: read operation at address " << address << " with length " << length << std::endl;
-  for(unsigned int i=address,num=0;i<address+length || i<SERVO_MEMORY_SIZE;i++,num++)
-    data[num]=rx28_servo_data[i];
-
-  return 0x00;
+  if(address<SERVO_MEMORY_SIZE)
+  {
+    std::cout << "do read" << std::endl;
+    for(unsigned int i=address,num=0;i<address+length && i<SERVO_MEMORY_SIZE;i++,num++)
+      data[num]=rx28_servo_data[i];
+    return 0x00;
+  }
+  else 
+    return 0x01;
 }
 
 unsigned char on_write_03(unsigned short int address, unsigned short int length, unsigned char *data)
 {
   std::cout << "servo 3: wrirte operation at address " << address << " with length " << length << std::endl;
 
-  for(unsigned int i=address,num=0;i<address+length || i<SERVO_MEMORY_SIZE;i++,num++)
+  for(unsigned int i=address,num=0;i<address+length && i<SERVO_MEMORY_SIZE;i++,num++)
     rx28_servo_data[i]=data[num];
 
   return 0x00;
@@ -47,17 +52,22 @@ void on_ping_01(void)
 unsigned char on_read_01(unsigned short int address, unsigned short int length, unsigned char *data)
 {
   std::cout << "servo 1: read operation at address " << address << " with length " << length << std::endl;
-  for(unsigned int i=address,num=0;i<address+length || i<SERVO_MEMORY_SIZE;i++,num++)
-    data[num]=rx28_servo_data[i];
-
-  return 0x00;
+  if(address<SERVO_MEMORY_SIZE)
+  {
+    std::cout << "do read" << std::endl;
+    for(unsigned int i=address,num=0;i<address+length && i<SERVO_MEMORY_SIZE;i++,num++)
+      data[num]=rx28_servo_data[i];
+    return 0x00;
+  }
+  else
+    return 0x01;
 }
 
 unsigned char on_write_01(unsigned short int address, unsigned short int length, unsigned char *data)
 {
   std::cout << "servo 1: wrirte operation at address " << address << " with length " << length << std::endl;
 
-  for(unsigned int i=address,num=0;i<address+length || i<SERVO_MEMORY_SIZE;i++,num++)
+  for(unsigned int i=address,num=0;i<address+length && i<SERVO_MEMORY_SIZE;i++,num++)
     rx28_servo_data[i]=data[num];
 
   return 0x00;