diff --git a/include/dynamixel.h b/include/dynamixel.h
index 22a42d8a2c1c70bab729ad64baeeec1faeed48ab..e842d55b38d8a5b233a707590aa791fc2263b2e6 100644
--- a/include/dynamixel.h
+++ b/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
*
diff --git a/include/dynamixelserver.h b/include/dynamixelserver.h
index 535cd35c08e5e6ebafecad3ccb6c0a391e37b6f6..c7266d36644bde1f495e0d2c386b2a4b48af8a5a 100644
--- a/include/dynamixelserver.h
+++ b/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
*
diff --git a/src/dynamixel_can.cpp b/src/dynamixel_can.cpp
index b403aba691eded67ee21f547d540bc9f12abbc6d..d023386cf78a4cb9bd94a6c5e11921738e135cbf 100644
--- a/src/dynamixel_can.cpp
+++ b/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()
diff --git a/src/dynamixelserver_can.cpp b/src/dynamixelserver_can.cpp
index 767f386d7c0d8170be0d235de59761b0d9697a42..8ebe02bee1f887a7f61a6ef7ec3baf39b9573d63 100644
--- a/src/dynamixelserver_can.cpp
+++ b/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)