diff --git a/include/dynamixel.h b/include/dynamixel.h
index 96c545cae70ec860f23f7876bdacc7ae4d4242c9..e842d55b38d8a5b233a707590aa791fc2263b2e6 100644
--- a/include/dynamixel.h
+++ b/include/dynamixel.h
@@ -1,7 +1,7 @@
#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;
- /**
- * \brief
- *
*/
- unsigned char node_address;
+ unsigned char baudrate_register;
/**
- * \brief mutual exclusion mechanism to access the usb
+ * \brief
*
*/
- CMutex *usb_access;
+ void set_baudrate(int baudrate);
+
+ 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);
+ void sync_packet_v2(unsigned char *data,unsigned int length,int *start);
/**
* \brief
*
*/
- void sync_packet_v2(unsigned char *data,unsigned int length,int *start);
+ void sync_packet_v1(unsigned char *data,unsigned int length,int *start);
+ /**
+ * \brief data reception event
+ *
+ */
+ 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
*
diff --git a/include/dynamixel_can.h b/include/dynamixel_can.h
new file mode 100644
index 0000000000000000000000000000000000000000..7ccc7311a7166c0666f764d9c904aa693acbee79
--- /dev/null
+++ b/include/dynamixel_can.h
@@ -0,0 +1,41 @@
+#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
diff --git a/include/dynamixelserver.h b/include/dynamixelserver.h
index 9c5559abda51b86b4510f97ca63b7f6ab4f28657..c7266d36644bde1f495e0d2c386b2a4b48af8a5a 100644
--- a/include/dynamixelserver.h
+++ b/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
*
@@ -243,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
*
diff --git a/include/dynamixelserver_can.h b/include/dynamixelserver_can.h
new file mode 100644
index 0000000000000000000000000000000000000000..26ea0b87f0e60e5b653678e2f19e21523b039084
--- /dev/null
+++ b/include/dynamixelserver_can.h
@@ -0,0 +1,126 @@
+#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
diff --git a/src/CMakeLists.txt b/src/CMakeLists.txt
index f325bc8eac42cee920082fa172e8a914a8831e58..363fbd0caa79c40479a36ff53c8d8814237d80bf 100644
--- a/src/CMakeLists.txt
+++ b/src/CMakeLists.txt
@@ -1,9 +1,9 @@
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)
diff --git a/src/dynamixel.cpp b/src/dynamixel.cpp
index 31f8ea8e1200b61d0b77d1b4c0239c79ae3d4ee3..029b0ed755714683aa979eb8cc269bf153286f11 100644
--- a/src/dynamixel.cpp
+++ b/src/dynamixel.cpp
@@ -1,7 +1,6 @@
+#include "dynamixel.h"
#include "eventexceptions.h"
-#include "dynamixelserver.h"
#include "dynamixelexceptions.h"
-#include "dynamixel.h"
#include "ftdiserver.h"
#include <iostream>
#include <sstream>
diff --git a/src/dynamixel_can.cpp b/src/dynamixel_can.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..d29ca29c260d49002c779b607a82b7acdf4df3d6
--- /dev/null
+++ b/src/dynamixel_can.cpp
@@ -0,0 +1,286 @@
+#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 *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)
+{
+ 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(this->rx_frame_id));
+ // read up to the length field
+ do{
+ if((num=((CCAN *)this->comm_dev)->get_num_data(this->rx_frame_id))==0)
+ {
+ this->event_server->wait_all(events,500);
+ num=((CCAN *)this->comm_dev)->get_num_data(this->rx_frame_id);
+ }
+ if((read+num)>1024)
+ {
+ ((CCAN *)this->comm_dev)->read(this->rx_frame_id,&data_int[read],1024-read);
+ read=1024;
+ }
+ else
+ {
+ if(num != 0)
+ ((CCAN *)this->comm_dev)->read(this->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_data(this->rx_frame_id))==0)
+ {
+ this->event_server->wait_all(events,500);
+ num=((CCAN *)this->comm_dev)->get_num_data(this->rx_frame_id);
+ }
+ if((read-start+num)>length)
+ {
+ ((CCAN *)this->comm_dev)->read(this->rx_frame_id,&data_int[read],length-read);
+ read=length;
+ }
+ else
+ {
+ if(num != 0)
+ ((CCAN *)this->comm_dev)->read(this->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)
+{
+ 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(this->rx_frame_id));
+ // read up to the length field
+ do{
+ if((num=((CCAN *)this->comm_dev)->get_num_data(this->rx_frame_id))==0)
+ {
+ this->event_server->wait_all(events,300);
+ num=((CCAN *)this->comm_dev)->get_num_data(this->rx_frame_id);
+ }
+ if((read+num)>1024)
+ {
+ ((CCAN *)this->comm_dev)->read(this->rx_frame_id,&data_int[read],1024-read);
+ read=1024;
+ }
+ else
+ {
+
+ if(num!=0)
+ ((CCAN *)this->comm_dev)->read(this->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_data(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(this->rx_frame_id,&data_int[read],1024-read);
+ read=1024;
+ }
+ else
+ {
+ if(num != 0)
+ ((CCAN *)this->comm_dev)->read(this->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()
+{
+
+}
+
+
diff --git a/src/dynamixelserver_can.cpp b/src/dynamixelserver_can.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..be8a152036ba76bb0e42ad649c83f4b755bc7c4c
--- /dev/null
+++ b/src/dynamixelserver_can.cpp
@@ -0,0 +1,430 @@
+#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 *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)
+{
+ 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(this->bus_info.rx_frame_id));
+ // read up to the length field
+ do{
+ if((num=((CCAN *)this->comm_dev)->get_num_data(this->bus_info.rx_frame_id))==0)
+ {
+ this->event_server->wait_all(events,200);
+ num=((CCAN *)this->comm_dev)->get_num_data(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_data(this->bus_info.rx_frame_id))==0)
+ {
+ this->event_server->wait_all(events,200);
+ num=((CCAN *)this->comm_dev)->get_num_data(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)
+{
+ 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(this->bus_info.rx_frame_id));
+ // read up to the length field
+ do{
+ if((num=((CCAN *)this->comm_dev)->get_num_data(this->bus_info.rx_frame_id))==0)
+ {
+ this->event_server->wait_all(events,200);
+ num=((CCAN *)this->comm_dev)->get_num_data(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
+ {
+ if(num != 0)
+ ((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_data(this->bus_info.rx_frame_id))==0)
+ {
+ this->event_server->wait_all(events,200);
+ num=((CCAN *)this->comm_dev)->get_num_data(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)
+{
+
+}
+
+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()
+{
+
+}
diff --git a/src/examples/CMakeLists.txt b/src/examples/CMakeLists.txt
index 666109205992fd850d7d764034bc95c210c91cd9..babdeeb17794cd780a11fdd0b9438af92aaf7920 100644
--- a/src/examples/CMakeLists.txt
+++ b/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)
diff --git a/src/examples/test_dynamixel_server_CAN.cpp b/src/examples/test_dynamixel_server_CAN.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..6693cba0aef84e5552f7d2322b35c129c2a43397
--- /dev/null
+++ b/src/examples/test_dynamixel_server_CAN.cpp
@@ -0,0 +1,78 @@
+#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;
+ }
+// }
+}