adding v2 protocol capabilities

include/dynamixel.h

@@ -38,11 +38,6 @@ class CDynamixel
      *
      */
     void set_baudrate(int baudrate);
-    /**
-     * \brief
-     *
-     */
-    void sync_packet_v2(unsigned char *data,unsigned int length,int *start);
 
   protected:
     /**
@@ -71,6 +66,11 @@ class CDynamixel
      */
     void handle_error(unsigned char error); 
   public:
+    /**
+     * \brief
+     *
+     */
+    void sync_packet_v2(unsigned char *data,unsigned int length,int *start);
     /**
      * \brief
      *

include/dynamixelserver.h

@@ -244,7 +244,7 @@ class CDynamixelServer
      * \brief 
      *
      */ 
-    CDynamixel *get_device(int dev_id,dyn_version_t version=dyn_version1);
+    virtual CDynamixel *get_device(int dev_id,dyn_version_t version=dyn_version1);
     /**
      * \brief 
      *

src/dynamixel_can.cpp

@@ -57,7 +57,49 @@ void CDynamixelCAN::send_instruction_packet_v1(dyn_inst_t inst,unsigned char *da
 
 void CDynamixelCAN::send_instruction_packet_v2(dyn_inst_t inst,unsigned char *data,unsigned short int len)
 {
+  unsigned char *packet=NULL;
+  int i,length,crc;
 
+  switch(inst)
+  {
+    case dyn_reg_write:
+    case dyn_write:
+    case dyn_read: length=10+len;
+                   packet=new unsigned char[length];
+                   break;
+    default: throw CDynamixelException(_HERE_,"Instruction not supported",this->get_id());
+             break;
+  }
+  packet[0]=0xFF;
+  packet[1]=0xFF;
+  packet[2]=0xFD;
+  packet[3]=0x00;
+  packet[4]=this->node_address;
+  packet[5]=(len+3)%256;
+  packet[6]=(len+3)/256;
+  packet[7]=inst;
+  for(i=0;i<len;i++)
+    packet[8+i]=data[i];
+  packet[length-2]=0x00;
+  packet[length-1]=0x00;
+  crc=CDynamixelServer::compute_checksum_v2(packet,length-2);
+  packet[length-2]=crc%256;
+  packet[length-1]=crc/256;
+  if(this->comm_dev!=NULL)
+  {
+    try{
+      ((CCAN *)this->comm_dev)->write(this->tx_frame_id,packet,length);
+      if(packet!=NULL)
+        delete[] packet;
+    }catch(CException &e){
+      throw e;
+    }
+  }
+  else
+  {
+    /* handle exceptions */
+    throw CDynamixelException(_HERE_,"The communication device is not properly configured",this->get_id());
+  }
 }
 
 unsigned char CDynamixelCAN::receive_status_packet_v1(unsigned char **data,unsigned char *len)
@@ -150,7 +192,85 @@ unsigned char CDynamixelCAN::receive_status_packet_v1(unsigned char **data,unsig
 
 unsigned char CDynamixelCAN::receive_status_packet_v2(unsigned char **data,unsigned short int *len)
 {
+  std::list<std::string> events;
+  unsigned char data_int[1024];
+  int num=0,read=0,length=0,start=0;
+  unsigned short int crc;
 
+  if(this->comm_dev!=NULL)
+  {
+    try{
+      events.push_back(((CCAN *)this->comm_dev)->get_new_frame_event_id());
+      // read up to the length field
+      do{
+        if((num=((CCAN *)this->comm_dev)->get_num_bytes(this->rx_frame_id))==0)
+        {
+          this->event_server->wait_all(events,100);
+          num=((CCAN *)this->comm_dev)->get_num_bytes(rx_frame_id);
+        }
+        if((read+num)>1024)
+        {
+          ((CCAN *)this->comm_dev)->read(rx_frame_id,&data_int[read],1024-read);
+          read=1024;
+        }
+        else
+        {
+          ((CCAN *)this->comm_dev)->read(rx_frame_id,&data_int[read],num);
+          read+=num;
+        }
+        this->sync_packet_v2(data_int,read,&start);
+      }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=((CCAN *)this->comm_dev)->get_num_bytes(this->rx_frame_id))==0)
+        {
+          this->event_server->wait_all(events,100);
+          num=this->comm_dev->get_num_data();
+        }
+        if((read-start+num)>1024)
+        {
+          ((CCAN *)this->comm_dev)->read(rx_frame_id,&data_int[read],1024-read);
+          read=1024;
+        }
+        else
+        {
+          ((CCAN *)this->comm_dev)->read(rx_frame_id,&data_int[read],num);
+          read+=num;
+        }
+      }
+      // check the checksum
+      crc=CDynamixelServer::compute_checksum_v2(&data_int[start],length-2);
+      if((crc%256)!=data_int[start+length-2] || (crc/256)!=data_int[start+length-1])
+      {
+        /* handle exceptions */
+        throw CDynamixelException(_HERE_,"Invalid Checksum",this->node_address);
+      }
+      // return the error
+      if(length>10)
+      {
+        *data=new unsigned char[length-11];
+        memcpy(*data,&data_int[start+9],length-11);
+        *len=length-11;
+      }
+      else
+      {
+        *data=NULL;
+        *len=0;
+      }
+      return data_int[start+8];
+    }catch(CEventTimeoutException &e){
+      throw e;
+    }catch(CException &e){
+      throw e;
+    }
+  }
+  else
+  {
+    /* handle exceptions */
+    throw CDynamixelException(_HERE_,"The communication device is not properly configured",this->node_address);
+  }
 }
 
 CDynamixelCAN::~CDynamixelCAN()

src/dynamixelserver_can.cpp

@@ -85,7 +85,59 @@ void CDynamixelServerCAN::send_instruction_packet_v1(dyn_inst_t inst,unsigned ch
 
 void CDynamixelServerCAN::send_instruction_packet_v2(dyn_inst_t inst,unsigned char *data,unsigned short int len,unsigned char id)
 {
+  unsigned char *packet=NULL;
+  int i,length,crc;
 
+  switch(inst)
+  {
+    case dyn_action: id=0xFE;// set the broadcast address
+    case dyn_ping: length=10;
+                   packet=new unsigned char[length];
+                   if(len!=0)
+                     throw CDynamixelServerException(_HERE_,"Invalid data length");
+                   break;
+    case dyn_bulk_read:
+    case dyn_bulk_write:
+    case dyn_sync_read:
+    case dyn_sync_write: id=0xFE;
+                         length=10+len;
+                         packet=new unsigned char[length];
+                         break;
+    default: throw CDynamixelServerException(_HERE_,"Instruction not supported");
+             break;
+  }
+  packet[0]=0xFF;
+  packet[1]=0xFF;
+  packet[2]=0xFD;
+  packet[3]=0x00;
+  packet[4]=id;
+  packet[5]=(len+3)%256;
+  packet[6]=(len+3)/256;
+  packet[7]=inst;
+  for(i=0;i<len;i++)
+    packet[8+i]=data[i];
+  packet[length-2]=0x00;
+  packet[length-1]=0x00;
+  crc=this->compute_checksum_v2(packet,length-2);
+  packet[length-2]=crc%256;
+  packet[length-1]=crc/256;
+  if(this->comm_dev!=NULL)
+  { 
+    try{
+      ((CCAN *)this->comm_dev)->write(this->bus_info.tx_frame_id,packet,length);
+      if(packet!=NULL)
+        delete[] packet;
+    }catch(CException &e){
+      throw e; 
+    }
+  }
+  else
+  {
+    if(packet!=NULL)
+      delete[] packet;
+    /* handle exceptions */
+    throw CDynamixelServerException(_HERE_,"The communication device is not properly configured");
+  }
 }
 
 unsigned char CDynamixelServerCAN::receive_status_packet_v1(unsigned char **data,unsigned char *len)
@@ -178,7 +230,85 @@ unsigned char CDynamixelServerCAN::receive_status_packet_v1(unsigned char **data
 
 unsigned char CDynamixelServerCAN::receive_status_packet_v2(unsigned char **data,unsigned short int *len)
 {
+  std::list<std::string> events;
+  unsigned char data_int[1024];
+  int num=0,read=0,length,start=0;
+  unsigned short int crc;
 
+  if(this->comm_dev!=NULL)
+  {
+    try{
+      events.push_back(((CCAN *)this->comm_dev)->get_new_frame_event_id());
+      // read up to the length field
+      do{
+        if((num=((CCAN *)this->comm_dev)->get_num_bytes(this->bus_info.rx_frame_id))==0)
+        {
+          this->event_server->wait_all(events,20);
+          num=((CCAN *)this->comm_dev)->get_num_bytes(this->bus_info.rx_frame_id);
+        }
+        if((read+num)>1024)
+        {
+          ((CCAN *)this->comm_dev)->read(this->bus_info.rx_frame_id,&data_int[read],1024-read);
+          read=1024;
+        }
+        else
+        {
+          ((CCAN *)this->comm_dev)->read(this->bus_info.rx_frame_id,&data_int[read],num);
+          read+=num;
+        }
+        this->sync_packet_v2(data_int,read,&start);
+      }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=((CCAN *)this->comm_dev)->get_num_bytes(this->bus_info.rx_frame_id))==0)
+        {
+          this->event_server->wait_all(events,20);
+          num=((CCAN *)this->comm_dev)->get_num_bytes(this->bus_info.rx_frame_id);
+        }
+        if((read-start+num)>length)
+        {
+          ((CCAN *)this->comm_dev)->read(this->bus_info.rx_frame_id,&data_int[read],length-read);
+          read=length;
+        }
+        else
+        {
+          ((CCAN *)this->comm_dev)->read(this->bus_info.rx_frame_id,&data_int[read],num);
+          read+=num;
+        }
+      }
+      // check the checksum
+      crc=this->compute_checksum_v2(&data_int[start],length-2);
+      if((crc%256)!=data_int[start+length-2] || (crc/256)!=data_int[start+length-1])
+      {
+        /* handle exceptions */
+        throw CDynamixelServerException(_HERE_,"Invalid Checksum");
+      }
+      // return the error
+      if(length>11)
+      {
+        *data=new unsigned char[length-11];
+        memcpy(*data,&data_int[start+9],length-11);
+        *len=length-11;
+      }
+      else
+      {
+        *data=NULL;
+        *len=0;  
+      }
+      return data_int[start+8];
+    }catch(CEventTimeoutException &e){
+      throw e;
+    }catch(CException &e){
+      throw e;
+    }
+  }
+  else
+  {
+    /* handle exceptions */
+    throw CDynamixelServerException(_HERE_,"The communication device is not properly configured");
+  }
 }
 
 void CDynamixelServerCAN::sync_packet_v1(unsigned char *data,unsigned int length,int *start)