dynamixel_can and dynamixelserver_can layers for v1 packets

include/dynamixel.h

 #ifndef _DYNAMIXEL_H
 #define _DYNAMIXEL_H
 
-//#include "dynamixelserver.h"
+#include "dynamixelserver.h"
 #include "dynamixel_common.h"
 #include "eventserver.h"
 #include "comm.h"
@@ -24,93 +24,93 @@ class CDynamixel
   private:
     friend class CDynamixelServer;
     /**
-     * \brief
-     * 
+     * \brief 
+     *
      */
-    CDynamixelServer *dyn_server;
+    unsigned char id_register;
     /**
-     * \brief Handle to the communications device
+     * \brief 
      *
-     */ 
-    CComm *comm_dev;
+     */
+    unsigned char baudrate_register;
     /**
-     * \brief
-     * 
+     * \brief 
+     *
      */
-    unsigned char node_address;
+    void set_baudrate(int baudrate);
     /**
-     * \brief mutual exclusion mechanism to access the usb
+     * \brief
      *
      */
-    CMutex *usb_access; 
+    void sync_packet_v2(unsigned char *data,unsigned int length,int *start);
+
+  protected:
     /**
-     * \brief Handle to the unique event server
+     * \brief 
      *
      */
-    CEventServer *event_server;
+    virtual void send_instruction_packet_v1(dyn_inst_t inst,unsigned char *data=NULL,unsigned char len=0);
     /**
-     * \brief data reception event
-     * 
+     * \brief 
+     *
      */
-    std::string usb_rx_event_id;
+    virtual void send_instruction_packet_v2(dyn_inst_t inst,unsigned char *data=NULL,unsigned short int len=0);
     /**
      * \brief 
      *
      */
-    unsigned char id_register;
+    virtual unsigned char receive_status_packet_v1(unsigned char **data,unsigned char *len);
     /**
      * \brief 
      *
      */
-    unsigned char baudrate_register;
+    virtual unsigned char receive_status_packet_v2(unsigned char **data,unsigned short int *len);
     /**
      * \brief 
      *
      */
-    void set_baudrate(int baudrate);
+    void handle_error(unsigned char error); 
+  public:
     /**
      * \brief
      *
      */
     void sync_packet_v1(unsigned char *data,unsigned int length,int *start);
     /**
-     * \brief
-     *
+     * \brief data reception event
+     * 
      */
-    void sync_packet_v2(unsigned char *data,unsigned int length,int *start);
+    std::string usb_rx_event_id;
     /**
      * \brief
      *
      */
     dyn_version_t version;
-
-  protected:
     /**
-     * \brief 
-     *
+     * \brief
+     * 
      */
-    void send_instruction_packet_v1(dyn_inst_t inst,unsigned char *data=NULL,unsigned char len=0);
+    unsigned char node_address;
     /**
-     * \brief 
+     * \brief mutual exclusion mechanism to access the usb
      *
      */
-    void send_instruction_packet_v2(dyn_inst_t inst,unsigned char *data=NULL,unsigned short int len=0);
+    CMutex *usb_access; 
     /**
-     * \brief 
-     *
+     * \brief
+     * 
      */
-    unsigned char receive_status_packet_v1(unsigned char **data,unsigned char *len);
+    CDynamixelServer *dyn_server;
     /**
-     * \brief 
+     * \brief Handle to the communications device
      *
-     */
-    unsigned char receive_status_packet_v2(unsigned char **data,unsigned short int *len);
+     */ 
+    CComm *comm_dev;
     /**
-     * \brief 
+     * \brief Handle to the unique event server
      *
      */
-    void handle_error(unsigned char error); 
-  public:
+    CEventServer *event_server;
     /**
      * \brief
      *

include/dynamixel_can.h

+#ifndef _DYNAMIXEL_CAN_H
+#define _DYNAMIXEL_CAN_H
+
+#include "dynamixel.h"
+#include "dynamixelserver_can.h"
+
+class CDynamixelCAN : public CDynamixel
+{
+  private:
+    friend class CDynamixelServerCAN;
+    unsigned int tx_frame_id;
+    unsigned int rx_frame_id;
+    dyn_inst_t current_inst;
+    unsigned int current_len;
+  protected:
+    /**
+     * \brief 
+     *
+     */
+    virtual void send_instruction_packet_v1(dyn_inst_t inst,unsigned char *data=NULL,unsigned char len=0);
+    /**
+     * \brief 
+     *
+     */
+    virtual void send_instruction_packet_v2(dyn_inst_t inst,unsigned char *data=NULL,unsigned short int len=0);
+    /**
+     * \brief 
+     *
+     */
+    virtual unsigned char receive_status_packet_v1(unsigned char **data,unsigned char *len);
+    /**
+     * \brief 
+     *
+     */
+    virtual unsigned char receive_status_packet_v2(unsigned char **data,unsigned short int *len);
+  public:
+    CDynamixelCAN(std::string &cont_id,unsigned int tx_frame_id,unsigned int rx_frame_id);
+    virtual ~CDynamixelCAN();
+};
+
+#endif

include/dynamixelserver.h

@@ -7,6 +7,7 @@
 #include "comm.h"
 #include "eventserver.h"
 #include "threadserver.h"
+#include "dynamixel_common.h"
 
 // forward declaration of the CDynamixel class
 class CDynamixel;
@@ -50,11 +51,6 @@ const int frequencies[9]={1000000,500000,400000,250000,200000,115200,57600,19200
 class CDynamixelServer
 {
   private:
-    /**
-     * \brief 
-     *
-     */ 
-    CEventServer *event_server;
     /**
      * \brief 
      *
@@ -99,22 +95,22 @@ class CDynamixelServer
      * \brief 
      *
      */ 
-    void send_instruction_packet_v1(dyn_inst_t inst,unsigned char *data=NULL,unsigned char len=0,unsigned char id=0xFE);
+    virtual void send_instruction_packet_v1(dyn_inst_t inst,unsigned char *data=NULL,unsigned char len=0,unsigned char id=0xFE);
     /**
      * \brief 
      *
      */ 
-    void send_instruction_packet_v2(dyn_inst_t inst,unsigned char *data=NULL,unsigned short int len=0,unsigned char id=0xFE);
+    virtual void send_instruction_packet_v2(dyn_inst_t inst,unsigned char *data=NULL,unsigned short int len=0,unsigned char id=0xFE);
     /**
      * \brief 
      *
      */ 
-    unsigned char receive_status_packet_v1(unsigned char **data,unsigned char *len);
+    virtual unsigned char receive_status_packet_v1(unsigned char **data,unsigned char *len);
     /**
      * \brief 
      *
      */ 
-    unsigned char receive_status_packet_v2(unsigned char **data,unsigned short int *len);
+    virtual unsigned char receive_status_packet_v2(unsigned char **data,unsigned short int *len);
     /**
      * \brief
      *
@@ -172,6 +168,11 @@ class CDynamixelServer
      */ 
     void handle_error(unsigned char error);
   public:
+    /**
+     * \brief 
+     *
+     */ 
+    CEventServer *event_server;
     /**
      * \brief 
      *

include/dynamixelserver_can.h

+#ifndef _DYNAMIXEL_SERVER_CAN_H
+#define _DYNAMIXEL_SERVER_CAN_H
+
+#include "dynamixelserver.h"
+
+/**
+ * \brief Basic Dynamixel bus information
+ *
+ * This structure holds the identifier of the current bus used (in case there
+ * exist multiple buses) and also the baudrate of the bus.  
+ */
+typedef struct
+{
+  std::string device;
+  unsigned int tx_frame_id;
+  unsigned int rx_frame_id;
+}TBus_info_can;
+
+/**
+ * \brief
+ *
+ */
+class CDynamixelServerCAN : public CDynamixelServer
+{
+  private:
+    std::string rx_event; 
+    dyn_inst_t current_inst;
+    unsigned int current_len;
+    /**
+     * \brief 
+     *
+     */
+    static CDynamixelServerCAN *pinstance;
+    /**
+     * \brief 
+     *
+     */ 
+    TBus_info_can bus_info;
+    /**
+     * \brief 
+     *
+     */
+    virtual void send_instruction_packet_v1(dyn_inst_t inst,unsigned char *data=NULL,unsigned char len=0,unsigned char id=0xFE);
+    /**
+     * \brief 
+     *
+     */
+    virtual void send_instruction_packet_v2(dyn_inst_t inst,unsigned char *data=NULL,unsigned short int len=0,unsigned char id=0xFE);
+    /**
+     * \brief 
+     *
+     */
+    virtual unsigned char receive_status_packet_v1(unsigned char **data,unsigned char *len);
+    /**
+     * \brief 
+     *
+     */
+    virtual unsigned char receive_status_packet_v2(unsigned char **data,unsigned short int *len);
+    /**
+     * \brief
+     *
+     */
+    virtual void sync_packet_v1(unsigned char *data,unsigned int length,int *start);
+    /**
+     * \brief
+     *
+     */
+    virtual void sync_packet_v2(unsigned char *data,unsigned int length,int *start);
+  protected:
+    /**
+     * \brief 
+     *
+     */ 
+    CDynamixelServerCAN();
+    /**
+     * \brief 
+     *
+     */ 
+    CDynamixelServerCAN(const CDynamixelServerCAN &object);
+    /**
+     * \brief 
+     *
+     */ 
+    CDynamixelServerCAN& operator = (const CDynamixelServerCAN &object);
+  public:
+    /**
+     * \brief 
+     *
+     */ 
+    static CDynamixelServerCAN* instance();
+    /**
+     * \brief 
+     *
+     */ 
+    void config_bus(const std::string &device,int baudrate,unsigned int tx_frame_id,unsigned int rx_frame_id);
+    /**
+     * \brief 
+     *
+     */ 
+    std::string &get_bus_device(void);
+    /**
+     * \brief 
+     *
+     */ 
+    void set_baudrate(int baudrate);
+    /**
+     * \brief 
+     *
+     */ 
+    int get_baudrate(void);
+#ifdef _HAVE_XSD
+    /**
+     * \brief 
+     *
+     */
+    virtual void config(std::string &filename);
+#endif
+    /**
+     * \brief 
+     *
+     */ 
+    virtual CDynamixel *get_device(int dev_id,dyn_version_t version=dyn_version1); 
+    virtual ~CDynamixelServerCAN();
+};
+
+#endif

src/CMakeLists.txt

 ADD_SUBDIRECTORY(xml)
 
 # edit the following line to add all the source code files of the library
-SET(sources dynamixel.cpp dynamixelserver.cpp dynamixelserver_ftdi.cpp dynamixelserver_serial.cpp dynamixelexceptions.cpp dynamixel_slave.cpp dynamixel_slave_ftdi.cpp dynamixel_slave_serial.cpp)
+SET(sources dynamixel.cpp dynamixelserver.cpp dynamixelserver_ftdi.cpp dynamixelserver_serial.cpp dynamixelserver_can.cpp dynamixel_can.cpp dynamixelexceptions.cpp dynamixel_slave.cpp dynamixel_slave_ftdi.cpp dynamixel_slave_serial.cpp)
 # edit the following line to add all the header files of the library
-SET(headers ../include/dynamixel.h ../include/dynamixelserver.h ../include/dynamixelserver_ftdi.h ../include/dynamixelserver_serial.h ../include/dynamixelexceptions.h ../include/dynamixel_common.h ../include/dynamixel_slave.h ../include/dynamixel_slave_ftdi.h ../include/dynamixel_slave_serial.h)
+SET(headers ../include/dynamixel.h ../include/dynamixelserver.h ../include/dynamixelserver_ftdi.h ../include/dynamixelserver_can.h ../include/dynamixel_can.h ../include/dynamixelserver_serial.h ../include/dynamixelexceptions.h ../include/dynamixel_common.h ../include/dynamixel_slave.h ../include/dynamixel_slave_ftdi.h ../include/dynamixel_slave_serial.h)
 
 INCLUDE_DIRECTORIES(. ../include)
 

src/dynamixel.cpp

+#include "dynamixel.h"
 #include "eventexceptions.h"
-#include "dynamixelserver.h"
 #include "dynamixelexceptions.h"
-#include "dynamixel.h"
 #include "ftdiserver.h"
 #include <iostream>
 #include <sstream>

src/dynamixel_can.cpp

+#include "dynamixel_can.h"
+#include "eventexceptions.h"
+#include "dynamixelexceptions.h"
+#include "can.h"
+#include "dynamixelserver_can.h"
+#include <iostream>
+
+CDynamixelCAN::CDynamixelCAN(std::string &cont_id,unsigned int tx_frame_id,unsigned int rx_frame_id) : CDynamixel(cont_id)
+{
+  this->tx_frame_id=tx_frame_id;
+  this->rx_frame_id=rx_frame_id;
+}
+
+void CDynamixelCAN::send_instruction_packet_v1(dyn_inst_t inst,unsigned char *data,unsigned char len)
+{
+  unsigned char *packet=NULL;
+  int i,length;
+
+  switch(inst)
+  {
+    case dyn_reg_write:
+    case dyn_write:
+    case dyn_read: length=6+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]=this->get_id();
+  packet[3]=len+2;
+  packet[4]=inst;
+  for(i=0;i<len;i++)
+    packet[5+i]=data[i];
+  // byte stuffing
+  packet[length-1]=0x00;
+  packet[length-1] = CDynamixelServer::compute_checksum_v1(packet,length);
+  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
+  {
+    if(packet!=NULL)
+      delete[] packet;
+    /* handle exceptions */
+    throw CDynamixelException(_HERE_,"The communication device is not properly configured",this->get_id());
+  }
+}
+
+void CDynamixelCAN::send_instruction_packet_v2(dyn_inst_t inst,unsigned char *data,unsigned short int len)
+{
+
+}
+
+unsigned char CDynamixelCAN::receive_status_packet_v1(unsigned char **data,unsigned char *len)
+{
+  std::list<std::string> events;
+  unsigned char data_int[1024];
+  int num=0,read=0,length,start=0;
+
+  if(this->current_inst==dyn_write)
+  {
+    *data=NULL;
+    *len=0;
+    return 0;
+  }
+  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_v1(data_int,read,&start);  
+      }while((read-start)<4);
+      length=data_int[start+3]+4;
+      // 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=((CCAN *)this->comm_dev)->get_num_bytes(this->rx_frame_id);
+        }
+        if((read-start+num)>length)
+        {
+          ((CCAN *)this->comm_dev)->read(rx_frame_id,&data_int[read],length-read);
+          read=length;
+        }
+        else
+        {
+          ((CCAN *)this->comm_dev)->read(rx_frame_id,&data_int[read],num);
+          read+=num;
+        }
+      }
+      // check the checksum
+      if(CDynamixelServer::compute_checksum_v1(&data_int[start],length)!=0x00)
+      {
+        /* handle exceptions */
+        throw CDynamixelException(_HERE_,"Invalid Checksum",this->node_address);
+      }
+      // byte destuffing
+      // return the error
+      if(length>6)
+      {
+        *data=new unsigned char[length-6];
+        memcpy(*data,&data_int[start+5],length-6);
+        *len=length-6;
+      }
+      else
+      {
+        *data=NULL;
+        *len=0;
+      }
+      return data_int[start+4];
+    }catch(CEventTimeoutException &e){
+      throw e;
+    }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_v2(unsigned char **data,unsigned short int *len)
+{
+
+}
+
+CDynamixelCAN::~CDynamixelCAN()
+{
+
+}
+
+

src/dynamixelserver_can.cpp

+#include "dynamixelserver_can.h"
+#include "dynamixel_can.h"
+#include "dynamixelexceptions.h"
+#include "eventexceptions.h"
+//#include "pal_can.h"
+#include "can.h"
+#include <sstream>
+#include <sys/types.h>
+#include <sys/stat.h>
+#include <unistd.h>
+#include <iostream>
+
+CDynamixelServerCAN *CDynamixelServerCAN::pinstance=NULL;
+
+CDynamixelServerCAN::CDynamixelServerCAN()
+{
+  this->bus_info.device="";
+  this->bus_info.tx_frame_id=-1;
+  this->bus_info.rx_frame_id=-1;
+  this->current_len=0;
+  this->current_inst=(dyn_inst_t)0x00;
+}
+
+CDynamixelServerCAN::CDynamixelServerCAN(const CDynamixelServerCAN& object)
+{
+
+}
+
+CDynamixelServerCAN& CDynamixelServerCAN::operator = (const CDynamixelServerCAN& object)
+{
+  return *this->pinstance;
+}
+
+void CDynamixelServerCAN::send_instruction_packet_v1(dyn_inst_t inst,unsigned char *data,unsigned char len,unsigned char id)
+{
+  unsigned char *packet=NULL;
+  int i,length;
+
+  switch(inst)
+  {
+    case dyn_action: id=0xFE;// set the broadcast address
+    case dyn_ping: length=6;
+                   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=6+len;
+                         packet=new unsigned char[length];
+                         break;
+    default: throw CDynamixelServerException(_HERE_,"Instruction not supported");
+             break;
+  }
+  packet[0]=0xFF;
+  packet[1]=0xFF;
+  packet[2]=id;
+  packet[3]=len+2;
+  packet[4]=inst;
+  for(i=0;i<len;i++)
+    packet[5+i]=data[i];
+  // byte stuffing   
+  packet[length-1]=0x00;   
+  packet[length-1]=this->compute_checksum_v1(packet,length);
+  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");
+  }
+}
+
+void CDynamixelServerCAN::send_instruction_packet_v2(dyn_inst_t inst,unsigned char *data,unsigned short int len,unsigned char id)
+{
+
+}
+
+unsigned char CDynamixelServerCAN::receive_status_packet_v1(unsigned char **data,unsigned char *len)
+{
+  std::list<std::string> events;
+  unsigned char data_int[1024];
+  int num=0,read=0,length,start=0;
+
+  if(this->current_inst==dyn_write)
+  {
+    *data=NULL;
+    *len=0;
+    return 0;
+  }
+  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_v1(data_int,read,&start);  
+      }while((read-start)<4);
+      length=data_int[start+3]+4;
+      // 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
+      if(CDynamixelServer::compute_checksum_v1(&data_int[start],length)!=0x00)
+      {
+        /* handle exceptions */
+        throw CDynamixelServerException(_HERE_,"Invalid Checksum");
+      }
+      // byte destuffing
+      // return the error
+      if(length>6)
+      {
+        *data=new unsigned char[length-6];
+        memcpy(*data,&data_int[start+5],length-6);
+        *len=length-6;
+      }
+      else
+      {
+        *data=NULL;
+        *len=0;
+      }
+      return data_int[start+4];
+    }catch(CEventTimeoutException &e){
+      throw e;
+    }catch(CException &e){
+      throw e;
+    }
+  }
+  else
+  {
+    /* handle exceptions */
+    throw CDynamixelServerException(_HERE_,"The communication device is not properly configured");
+  }
+}
+
+unsigned char CDynamixelServerCAN::receive_status_packet_v2(unsigned char **data,unsigned short int *len)
+{
+
+}
+
+void CDynamixelServerCAN::sync_packet_v1(unsigned char *data,unsigned int length,int *start)
+{
+
+}
+
+void CDynamixelServerCAN::sync_packet_v2(unsigned char *data,unsigned int length,int *start)
+{
+
+}
+
+CDynamixelServerCAN *CDynamixelServerCAN::instance(void)
+{
+  if (CDynamixelServerCAN::pinstance == NULL)
+  {
+    CDynamixelServerCAN::pinstance = new CDynamixelServerCAN(); // Creamos la instancia
+  }
+  return CDynamixelServerCAN::pinstance; // Retornamos la dirección de la instancia
+}
+
+void CDynamixelServerCAN::config_bus(const std::string &device, int baudrate,unsigned int tx_frame_id,unsigned int rx_frame_id)
+{
+  std::size_t pos;
+  std::string can_name;
+
+  if((pos=device.find_last_of("/"))==std::string::npos)
+    can_name="dynamixel_can_"+device;
+  else
+    can_name="dynamixel_can_"+device.substr(pos+1);
+  this->comm_dev=(CComm *)new CCAN(can_name);
+  ((CCAN *)this->comm_dev)->open(device);
+  ((CCAN *)this->comm_dev)->add_id_filter(rx_frame_id,0xFFF);
+  this->bus_info.device=device;
+  this->bus_info.tx_frame_id=tx_frame_id;
+  this->bus_info.rx_frame_id=rx_frame_id;
+}
+
+#ifdef _HAVE_XSD
+void CDynamixelServerCAN::config(std::string &filename)
+{
+}
+#endif
+
+std::string &CDynamixelServerCAN::get_bus_device(void)
+{
+  return this->bus_info.device;
+}
+
+void CDynamixelServerCAN::set_baudrate(int baudrate)
+{
+
+}
+
+int CDynamixelServerCAN::get_baudrate(void)
+{
+  return 0;
+}
+
+CDynamixel *CDynamixelServerCAN::get_device(int dev_id,dyn_version_t version)
+{
+  std::stringstream device_name;
+  CDynamixelCAN *dynamixel=NULL;
+  std::string name,serial;
+  bool updated=false;
+  unsigned int i=0;
+
+  this->dynamixel_access.enter();
+  if(this->comm_dev==NULL)
+  {
+    this->dynamixel_access.exit();
+    /* handle exceptions */
+    throw CDynamixelServerException(_HERE_,"No communication device has been initialized.");
+  }
+  this->dynamixel_access.exit();
+  if(dynamixel==NULL)
+  {
+    this->dynamixel_access.enter();
+    device_name.str("");
+    device_name << "dynamixel_bus_" << this->comm_dev->get_id() << "_dev_" << dev_id << "_v" << version;
+    name=device_name.str();
+    dynamixel=new CDynamixelCAN(name,this->bus_info.tx_frame_id,this->bus_info.rx_frame_id);
+    dynamixel->dyn_server=this;
+    dynamixel->comm_dev=this->comm_dev;
+    dynamixel->usb_access=&this->dynamixel_access;
+    dynamixel->node_address=dev_id;
+    dynamixel->version=version;
+    dynamixel->usb_rx_event_id=this->comm_dev->get_rx_event_id();
+    if(version==dyn_version1)
+    {
+      for(i=0;i<devices_v1.size();i++)
+      {
+        if(this->devices_v1[i]==dev_id)
+          updated=true;
+      }
+      if(!updated)
+        this->devices_v1.push_back(dev_id);
+    }
+    else
+    {
+      for(i=0;i<devices_v2.size();i++)
+      {
+        if(this->devices_v2[i]==dev_id)
+          updated=true;
+      }
+      if(!updated)
+        this->devices_v2.push_back(dev_id);
+    }
+  }
+  this->dynamixel_access.exit();
+
+  return dynamixel;
+}
+
+CDynamixelServerCAN::~CDynamixelServerCAN()
+{
+ 
+}

src/examples/CMakeLists.txt

@@ -4,6 +4,12 @@ ADD_EXECUTABLE(test_dynamixel_server test_dynamixel_server.cpp)
 # edit the following line to add the necessary libraries
 TARGET_LINK_LIBRARIES(test_dynamixel_server dynamixel)
 
+# edit the following line to add the source code for the example and the name of the executable
+ADD_EXECUTABLE(test_dynamixel_server_CAN test_dynamixel_server_CAN.cpp)
+
+# edit the following line to add the necessary libraries
+TARGET_LINK_LIBRARIES(test_dynamixel_server_CAN dynamixel)
+
 # edit the following line to add the source code for the example and the name of the executable
 ADD_EXECUTABLE(test_dynamixel_server_no_scan test_dynamixel_server_no_scan.cpp)
 

src/examples/test_dynamixel_server_CAN.cpp

+#include "eventexceptions.h"
+#include "dynamixelserver_ftdi.h"
+#include <iostream>
+#include "dynamixelserver_can.h"
+#include "dynamixel_can.h"
+
+std::string serial="slcan0";
+
+int main(int argc, char *argv[])
+{
+  CDynamixelServerCAN *dyn_server= CDynamixelServerCAN::instance();
+  CEventServer *event_server=CEventServer::instance();
+  int num_buses=0,baudrate=0,event_id;
+  std::list<std::string> events;
+  std::vector<int> devices;
+  unsigned short int model;
+  CDynamixelCAN *dyn_device;
+  unsigned int i,num_dev;
+
+  try{
+   events.push_back(dyn_server->get_scan_done_event_id());
+   events.push_back(dyn_server->get_scan_error_event_id());
+   dyn_server->config_bus(serial,1000000,0x33C,0x5BC); 
+   dyn_server->start_scan();
+   event_id=event_server->wait_first(events);
+   if(event_id==0)
+   {
+     num_dev=dyn_server->get_num_devices();
+     std::cout << "Num. devices: " << num_dev << std::endl;
+     baudrate=dyn_server->get_baudrate();
+     std::cout << "Baudrate: " << baudrate << " bps" << std::endl;
+     devices=dyn_server->get_device_ids();
+     for(i=1;i<num_dev;i++)
+     {
+       std::cout << "servo " << devices[i] << std::endl;
+       dyn_device=(CDynamixelCAN*)dyn_server->get_device(devices[i]);
+       dyn_device->read_word_register(0x00,&model);
+       std::cout << "servo " << devices[i] << " model " << (int)model << std::endl;
+       dyn_server->free_device(devices[i]);
+       delete dyn_device;
+     }
+   }
+   else
+     std::cout << "Error while scanning the bus: " << dyn_server->get_scan_error() << std::endl;
+//   try{
+//     dyn_server->config_bus(0,1000000);
+//     dyn_server->start_scan();
+//     event_server->wait_first(events,2000);
+//    }catch(CException &e){
+//      dyn_server->stop_scan();
+//      std::cout << "Scanning canceled !!!" << std::endl;
+//    }
+//   /* scan for version 2 devices */
+//   dyn_server->config_bus(0,1000000);
+//   dyn_server->start_scan(dyn_version2);
+//   event_id=event_server->wait_first(events);
+//   if(event_id==0)
+//   {
+//     num_dev=dyn_server->get_num_devices(dyn_version2);
+//     std::cout << "Num. devices: " << num_dev << std::endl;
+//     baudrate=dyn_server->get_baudrate();
+//     std::cout << "Baudrate: " << baudrate << " bps" << std::endl;
+//     devices=dyn_server->get_device_ids(dyn_version2);
+//     for(i=0;i<num_dev;i++)
+//     {
+//       dyn_device=dyn_server->get_device(devices[i],dyn_version2);
+//       dyn_device->read_word_register(0x00,&model);
+//       dyn_server->free_device(devices[i],dyn_version2);
+//       delete dyn_device;
+//     }
+//   }
+//   else
+//     std::cout << "Error while scanning the bus: " << dyn_server->get_scan_error() << std::endl;
+   }catch(CException &e){
+     std::cout << e.what() << std::endl;
+   }
+// }
+}