Added functions to read from and write to an arbitrary length of the internal memory of the device.

src/dynamixel.cpp

@@ -629,6 +629,208 @@ void CDynamixel::registered_word_write(unsigned char address, unsigned short int
   }
 }
 
+void CDynamixel::write_registers(unsigned char address, unsigned char *data, unsigned int length)
+{
+  unsigned char *packet;
+  std::list<std::string> events;
+  unsigned char answer[6];
+  std::string error_msg;
+  int num=0,read=0,it=0;
+  bool rx_tx_ok=false;
+  unsigned int i=0;
+
+  if(this->usb_dev!=NULL)
+  {
+    if(address+length>255)
+    {
+      /* handle exceptions */
+      throw CDynamixelException(_HERE_,"Invalid address range",this->node_address);
+    }
+    else
+    {
+      events.push_back(this->usb_rx_event_id); 
+      while(!rx_tx_ok)
+      { 
+        packet=new unsigned char[length+7];
+        this->usb_access->enter();
+        packet[0]=0xFF;
+        packet[1]=0xFF;
+        packet[2]=this->node_address;
+        packet[3]=length+3;
+        packet[4]=0x03;
+        packet[5]=(unsigned char)address;
+        for(i=0;i<length;i++)
+          packet[i+6]=data[i];
+        packet[length+6]=this->compute_checksum(packet,length+6);
+  
+        if(this->usb_dev->write(packet,length+7)!=(length+7))
+        {
+          /* handle exceptions */
+          this->usb_access->exit();
+          delete[] packet;
+          throw CDynamixelException(_HERE_,"Unexpected error while writing to the dynamixel device",this->node_address);
+        }
+        else
+        {  
+          delete[] packet;
+          try{
+            while(read<6)
+            {
+              this->event_server->wait_all(events,100);
+              num=this->usb_dev->get_num_data();
+              this->usb_dev->read(&answer[read],num);
+              read+=num;
+            }
+            if(this->compute_checksum(answer,6)!=0)
+            {
+              /* handle exceptions */
+              this->usb_access->exit();
+              it++;
+              if(it==NUM_RETRIES)
+                throw CDynamixelSyncException("Invalid packet checksum");
+              else
+                read=0;
+            }
+            else
+            {
+              rx_tx_ok=true;// transmission ok
+              this->usb_access->exit(); 
+            }
+          }catch(CEventTimeoutException &e){
+            this->resync();
+            this->usb_access->exit();
+          }
+        }
+      }
+      if(answer[4]!=0x00)
+      {
+        /* handle exceptions */
+        if(answer[4]&0x01)
+          error_msg+="\nThe supply voltage is out of range";
+        if(answer[4]&0x02)
+          error_msg+="\nThe goal position is out of range";
+        if(answer[4]&0x04)
+          error_msg+="\nThe internal temperature is too high";
+        if(answer[4]&0x08)
+          error_msg+="\nThe provided parameter is out of range";
+        if(answer[4]&0x10)
+          error_msg+="\nThe packet sent to the dynamixel device had an invalid checksum";
+        if(answer[4]&0x20)
+          error_msg+="\nOverload";
+        if(answer[4]&0x40)
+          error_msg+="\nInvalid instruction";
+        throw CDynamixelAlarmException(_HERE_,error_msg,this->node_address,answer[4]);
+      }
+    }
+  }
+  else
+  {
+    /* handle exceptions */
+    throw CDynamixelException(_HERE_,"The communication device is not ready to send data",this->node_address);
+  }
+}
+
+void CDynamixel::read_registers(unsigned char address, unsigned char *data, unsigned int length)
+{
+  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;
+  std::string error_msg;
+  bool rx_tx_ok=false;
+  unsigned int i=0;
+
+  if(this->usb_dev!=NULL)
+  {
+    events.push_back(this->usb_rx_event_id); 
+    while(!rx_tx_ok)
+    {
+      this->usb_access->enter();
+      packet[2]=this->node_address;
+      packet[5]=(unsigned char)address;
+      packet[6]=length;
+      packet[7]=this->compute_checksum(packet,8);
+
+      if(this->usb_dev->write(packet,8)!=8)
+      {
+        /* handle exceptions */
+        this->usb_access->exit();
+        throw CDynamixelException(_HERE_,"Unexpected error while writing to the dynamixel device",this->node_address);
+      }
+      else
+      {
+        try{
+          answer=new unsigned char[length+6];
+          while(read<read_length)
+          {
+            this->event_server->wait_all(events,100);
+            num=this->usb_dev->get_num_data();
+            this->usb_dev->read(&answer[read],num);
+            read+=num;
+            if(read>5 && answer[4]!=0x00)
+              read_length=6;
+          }
+          if(this->compute_checksum(answer,read_length)!=0)
+          {
+            /* handle exceptions */
+            this->usb_access->exit();
+            it++;
+            if(it==NUM_RETRIES)
+            {
+              delete[] answer;
+              throw CDynamixelSyncException("Invalid packet checksum");
+            }
+            else
+            {
+              read=0;
+              read_length=length+6;
+            }
+          }
+          else 
+          { 
+            rx_tx_ok=true;// transmission ok
+            this->usb_access->exit();
+          }
+        }catch(CEventTimeoutException &e){
+          this->resync();
+          this->usb_access->exit();
+        }
+      }
+    }
+    if(answer[4]!=0x00)
+    {
+      /* handle exceptions */
+      if(answer[4]&0x01)
+        error_msg+="\nThe supply voltage is out of range";
+      if(answer[4]&0x02)
+        error_msg+="\nThe goal position is out of range";
+      if(answer[4]&0x04)
+        error_msg+="\nThe internal temperature is too high";
+      if(answer[4]&0x08)
+        error_msg+="\nThe provided parameter is out of range";
+      if(answer[4]&0x10)
+        error_msg+="\nThe packet sent to the dynamixel device had an invalid checksum";
+      if(answer[4]&0x20)
+        error_msg+="\nOverload";
+      if(answer[4]&0x40)
+        error_msg+="\nInvalid instruction";
+      delete[] answer;
+      throw CDynamixelAlarmException(_HERE_,error_msg,this->node_address,answer[4]);
+    }
+    else
+    {
+      for(i=0;i<length;i++)
+        data[i]=answer[i+5];
+      delete[] answer;
+    }
+  }
+  else
+  {
+    /* handle exceptions */
+    throw CDynamixelException(_HERE_,"The communication device is not ready to send data",this->node_address);
+  }
+}
+
 void CDynamixel::reset(void)
 {
   unsigned char packet[6]={0xFF,0xFF,0x00,0x02,0x06,0x00};

src/dynamixel.h

@@ -106,6 +106,16 @@ class CDynamixel
      *
      */
     void registered_word_write(unsigned char address, unsigned short int data);
+    /**
+     * \brief Function to write to a register
+     *
+     */
+    void write_registers(unsigned char address, unsigned char *data, unsigned int length);
+    /**
+     * \brief Function to write to a register
+     *
+     */
+    void read_registers(unsigned char address, unsigned char *data, unsigned int length);
     /**
      * \brief 
      *