diff --git a/src/dynamixel.cpp b/src/dynamixel.cpp
index 5b4d047cc40ba129062a8522ffd297bdbd2dc47d..7ddd9471fcd0fa78e48fc103d82ffac8e85aba88 100644
--- a/src/dynamixel.cpp
+++ b/src/dynamixel.cpp
@@ -46,14 +46,11 @@ void CDynamixel::send_instruction_packet_v1(dyn_inst_t inst,unsigned char *data,
packet[length-1]=CDynamixelServer::compute_checksum_v1(packet,length);
if(this->comm_dev!=NULL)
{
- this->usb_access->enter();
if(this->comm_dev->write(packet,length)!=length)
{
/* handle exceptions */
- this->usb_access->exit();
throw CDynamixelException(_HERE_,"Unexpected error while writing to the communication device",this->node_address);
}
- this->usb_access->exit();
}
else
{
@@ -94,14 +91,11 @@ void CDynamixel::send_instruction_packet_v2(dyn_inst_t inst,unsigned char *data,
packet[length-1]=crc/256;
if(this->comm_dev!=NULL)
{
- this->usb_access->enter();
if(this->comm_dev->write(packet,length)!=length)
{
/* handle exceptions */
- this->usb_access->exit();
throw CDynamixelException(_HERE_,"Unexpected error while writing to the communication device",this->node_address);
}
- this->usb_access->exit();
}
else
{
@@ -119,7 +113,6 @@ unsigned char CDynamixel::receive_status_packet_v1(unsigned char **data,unsigned
if(this->comm_dev!=NULL)
{
try{
- this->usb_access->enter();
events.push_back(this->comm_dev->get_rx_event_id());
// read up to the length field
do{
@@ -163,7 +156,6 @@ unsigned char CDynamixel::receive_status_packet_v1(unsigned char **data,unsigned
// check the checksum
if(CDynamixelServer::compute_checksum_v1(&data_int[start],length)!=0x00)
{
- this->usb_access->exit();
/* handle exceptions */
throw CDynamixelException(_HERE_,"Invalid Checksum",this->node_address);
}
@@ -180,13 +172,10 @@ unsigned char CDynamixel::receive_status_packet_v1(unsigned char **data,unsigned
*data=NULL;
*len=0;
}
- this->usb_access->exit();
return data_int[start+4];
}catch(CEventTimeoutException &e){
- this->usb_access->exit();
throw e;
}catch(CException &e){
- this->usb_access->exit();
throw e;
}
}
@@ -207,7 +196,6 @@ unsigned char CDynamixel::receive_status_packet_v2(unsigned char **data,unsigned
if(this->comm_dev!=NULL)
{
try{
- this->usb_access->enter();
events.push_back(this->comm_dev->get_rx_event_id());
// read up to the length field
do{
@@ -252,7 +240,6 @@ unsigned char CDynamixel::receive_status_packet_v2(unsigned char **data,unsigned
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])
{
- this->usb_access->exit();
/* handle exceptions */
throw CDynamixelException(_HERE_,"Invalid Checksum",this->node_address);
}
@@ -268,13 +255,10 @@ unsigned char CDynamixel::receive_status_packet_v2(unsigned char **data,unsigned
*data=NULL;
*len=0;
}
- this->usb_access->exit();
return data_int[start+8];
}catch(CEventTimeoutException &e){
- this->usb_access->exit();
throw e;
}catch(CException &e){
- this->usb_access->exit();
throw e;
}
}
@@ -434,37 +418,6 @@ void CDynamixel::sync_packet_v2(unsigned char *data,unsigned int length,int *sta
}
}
-
-void CDynamixel::resync(void)
-{
- std::list<std::string> events;
- unsigned char foo=0x00,*data;
- bool synced=false;
- int num=0,it=0;
-
- events.push_back(this->usb_rx_event_id);
- // write to the dynamixel bus until it returns a packet
- do{
- try{
- this->comm_dev->write(&foo,1);
- this->event_server->wait_all(events,500);
- num=this->comm_dev->get_num_data();
- data=new unsigned char[num];
- this->comm_dev->read(data,num);
- delete[] data;
- synced=true;
- }catch(CEventTimeoutException &e){
- it++;
- synced=false;
- }
- }while(!synced && it<NUM_RETRIES);
- if(it==NUM_RETRIES)
- {
- this->usb_access->exit();
- throw CDynamixelSyncException("Exceeded maximum number of retries");
- }
-}
-
void CDynamixel::read_byte_register(unsigned short int address,unsigned char *value)
{
unsigned char *data,error,length_v1,cmd[4];
@@ -472,27 +425,44 @@ void CDynamixel::read_byte_register(unsigned short int address,unsigned char *va
if(address!=(unsigned short int)-1)
{
- if(version==dyn_version1)
+ try
{
- cmd[0]=address%256;
- cmd[1]=1;
- this->send_instruction_packet_v1(dyn_read,cmd,2);
- error=this->receive_status_packet_v1(&data,&length_v1);
+ this->usb_access->enter();
+ if(version==dyn_version1)
+ {
+ cmd[0]=address%256;
+ cmd[1]=1;
+ this->send_instruction_packet_v1(dyn_read,cmd,2);
+ error=this->receive_status_packet_v1(&data,&length_v1);
+ }
+ else
+ {
+ cmd[0]=address%256;
+ cmd[1]=address/256;
+ cmd[2]=1;
+ cmd[3]=0;
+ this->send_instruction_packet_v2(dyn_read,cmd,4);
+ error=this->receive_status_packet_v2(&data,&length_v2);
+ }
+ this->usb_access->exit();
}
- else
+ catch(CEventTimeoutException &e)
{
- cmd[0]=address%256;
- cmd[1]=address/256;
- cmd[2]=1;
- cmd[3]=0;
- this->send_instruction_packet_v2(dyn_read,cmd,4);
- error=this->receive_status_packet_v2(&data,&length_v2);
+ this->usb_access->exit();
+ throw e;
+ }
+ catch(CException &e)
+ {
+ this->usb_access->exit();
+ throw e;
}
try{
this->handle_error(error);
- *value=data[0];
- if(data!=NULL)
+ if (data != NULL)
+ {
+ *value=data[0];
delete[] data;
+ }
}catch(CException &e){
if(data!=NULL)
delete[] data;
@@ -508,27 +478,44 @@ void CDynamixel::read_word_register(unsigned short int address,unsigned short in
if(address!=(unsigned short int)-1)
{
- if(version==dyn_version1)
+ try
{
- cmd[0]=address%256;
- cmd[1]=2;
- this->send_instruction_packet_v1(dyn_read,cmd,2);
- error=this->receive_status_packet_v1(&data,&length_v1);
+ this->usb_access->enter();
+ if(version==dyn_version1)
+ {
+ cmd[0]=address%256;
+ cmd[1]=2;
+ this->send_instruction_packet_v1(dyn_read,cmd,2);
+ error=this->receive_status_packet_v1(&data,&length_v1);
+ }
+ else
+ {
+ cmd[0]=address%256;
+ cmd[1]=address/256;
+ cmd[2]=2;
+ cmd[3]=0;
+ this->send_instruction_packet_v2(dyn_read,cmd,4);
+ error=this->receive_status_packet_v2(&data,&length_v2);
+ }
+ this->usb_access->exit();
}
- else
+ catch(CEventTimeoutException &e)
{
- cmd[0]=address%256;
- cmd[1]=address/256;
- cmd[2]=2;
- cmd[3]=0;
- this->send_instruction_packet_v2(dyn_read,cmd,4);
- error=this->receive_status_packet_v2(&data,&length_v2);
+ this->usb_access->exit();
+ throw e;
+ }
+ catch(CException &e)
+ {
+ this->usb_access->exit();
+ throw e;
}
try{
this->handle_error(error);
- *value=data[0]+data[1]*256;
- if(data!=NULL)
+ if (data != NULL)
+ {
+ *value=data[0]+data[1]*256;
delete[] data;
+ }
}catch(CException &e){
if(data!=NULL)
delete[] data;
@@ -544,24 +531,39 @@ void CDynamixel::write_byte_register(unsigned short int address, unsigned char v
if(address!=(unsigned short int)-1)
{
- if(version==dyn_version1)
+ try
{
- cmd[0]=address%256;
- cmd[1]=value;
- this->send_instruction_packet_v1(dyn_write,cmd,2);
- error=this->receive_status_packet_v1(&data,&length_v1);
+ this->usb_access->enter();
+ if(version==dyn_version1)
+ {
+ cmd[0]=address%256;
+ cmd[1]=value;
+ this->send_instruction_packet_v1(dyn_write,cmd,2);
+ error=this->receive_status_packet_v1(&data,&length_v1);
+ }
+ else
+ {
+ cmd[0]=address%256;
+ cmd[1]=address/256;
+ cmd[2]=value;
+ this->send_instruction_packet_v2(dyn_write,cmd,3);
+ error=this->receive_status_packet_v2(&data,&length_v2);
+ }
+ if(data!=NULL)
+ delete[] data;
+ this->handle_error(error);
+ this->usb_access->exit();
}
- else
+ catch(CEventTimeoutException &e)
{
- cmd[0]=address%256;
- cmd[1]=address/256;
- cmd[2]=value;
- this->send_instruction_packet_v2(dyn_write,cmd,3);
- error=this->receive_status_packet_v2(&data,&length_v2);
+ this->usb_access->exit();
+ throw e;
+ }
+ catch(CException &e)
+ {
+ this->usb_access->exit();
+ throw e;
}
- if(data!=NULL)
- delete[] data;
- this->handle_error(error);
}
}
@@ -572,26 +574,41 @@ void CDynamixel::write_word_register(unsigned short int address, unsigned short
if(address!=(unsigned short int)-1)
{
- if(version==dyn_version1)
+ try
{
- cmd[0]=address%256;
- cmd[1]=value%256;
- cmd[2]=value/256;
- this->send_instruction_packet_v1(dyn_write,cmd,3);
- error=this->receive_status_packet_v1(&data,&length_v1);
+ this->usb_access->enter();
+ if(version==dyn_version1)
+ {
+ cmd[0]=address%256;
+ cmd[1]=value%256;
+ cmd[2]=value/256;
+ this->send_instruction_packet_v1(dyn_write,cmd,3);
+ error=this->receive_status_packet_v1(&data,&length_v1);
+ }
+ else
+ {
+ cmd[0]=address%256;
+ cmd[1]=address/256;
+ cmd[2]=value%256;
+ cmd[3]=value/256;
+ this->send_instruction_packet_v2(dyn_write,cmd,4);
+ error=this->receive_status_packet_v2(&data,&length_v2);
+ }
+ if(data!=NULL)
+ delete[] data;
+ this->handle_error(error);
+ this->usb_access->exit();
}
- else
+ catch(CEventTimeoutException &e)
{
- cmd[0]=address%256;
- cmd[1]=address/256;
- cmd[2]=value%256;
- cmd[3]=value/256;
- this->send_instruction_packet_v2(dyn_write,cmd,4);
- error=this->receive_status_packet_v2(&data,&length_v2);
+ this->usb_access->exit();
+ throw e;
+ }
+ catch(CException &e)
+ {
+ this->usb_access->exit();
+ throw e;
}
- if(data!=NULL)
- delete[] data;
- this->handle_error(error);
}
}
@@ -602,24 +619,39 @@ void CDynamixel::registered_byte_write(unsigned short int address, unsigned char
if(address!=(unsigned short int)-1)
{
- if(version==dyn_version1)
+ try
{
- cmd[0]=address%256;
- cmd[1]=value;
- this->send_instruction_packet_v1(dyn_reg_write,cmd,2);
- error=this->receive_status_packet_v1(&data,&length_v1);
+ this->usb_access->enter();
+ if(version==dyn_version1)
+ {
+ cmd[0]=address%256;
+ cmd[1]=value;
+ this->send_instruction_packet_v1(dyn_reg_write,cmd,2);
+ error=this->receive_status_packet_v1(&data,&length_v1);
+ }
+ else
+ {
+ cmd[0]=address%256;
+ cmd[1]=address/256;
+ cmd[2]=value;
+ this->send_instruction_packet_v2(dyn_reg_write,cmd,3);
+ error=this->receive_status_packet_v2(&data,&length_v2);
+ }
+ if(data!=NULL)
+ delete[] data;
+ this->handle_error(error);
+ this->usb_access->exit();
}
- else
- {
- cmd[0]=address%256;
- cmd[1]=address/256;
- cmd[2]=value;
- this->send_instruction_packet_v2(dyn_reg_write,cmd,3);
- error=this->receive_status_packet_v2(&data,&length_v2);
+ catch(CEventTimeoutException &e)
+ {
+ this->usb_access->exit();
+ throw e;
+ }
+ catch(CException &e)
+ {
+ this->usb_access->exit();
+ throw e;
}
- if(data!=NULL)
- delete[] data;
- this->handle_error(error);
}
}
@@ -630,26 +662,41 @@ void CDynamixel::registered_word_write(unsigned short int address, unsigned shor
if(address==(unsigned short int)-1)
{
- if(version==dyn_version1)
+ try
{
- cmd[0]=address%256;
- cmd[1]=value%256;
- cmd[2]=value/256;
- this->send_instruction_packet_v1(dyn_reg_write,cmd,3);
- error=this->receive_status_packet_v1(&data,&length_v1);
+ this->usb_access->enter();
+ if(version==dyn_version1)
+ {
+ cmd[0]=address%256;
+ cmd[1]=value%256;
+ cmd[2]=value/256;
+ this->send_instruction_packet_v1(dyn_reg_write,cmd,3);
+ error=this->receive_status_packet_v1(&data,&length_v1);
+ }
+ else
+ {
+ cmd[0]=address%256;
+ cmd[1]=address/256;
+ cmd[2]=value%256;
+ cmd[3]=value/256;
+ this->send_instruction_packet_v2(dyn_reg_write,cmd,4);
+ error=this->receive_status_packet_v2(&data,&length_v2);
+ }
+ if(data!=NULL)
+ delete[] data;
+ this->handle_error(error);
+ this->usb_access->exit();
}
- else
+ catch(CEventTimeoutException &e)
{
- cmd[0]=address%256;
- cmd[1]=address/256;
- cmd[2]=value%256;
- cmd[3]=value/256;
- this->send_instruction_packet_v2(dyn_reg_write,cmd,4);
- error=this->receive_status_packet_v2(&data,&length_v2);
+ this->usb_access->exit();
+ throw e;
+ }
+ catch(CException &e)
+ {
+ this->usb_access->exit();
+ throw e;
}
- if(data!=NULL)
- delete[] data;
- this->handle_error(error);
}
}
@@ -660,30 +707,45 @@ void CDynamixel::registered_write(unsigned short int address, unsigned char *val
if(address!=(unsigned short int)-1)
{
- if(version==dyn_version1)
+ try
{
- cmd=new unsigned char[length+1];
- cmd[0]=address%256;
- for(i=0;i<length;i++)
- cmd[1+i]=values[i];
- this->send_instruction_packet_v1(dyn_reg_write,cmd,length+1);
- delete[] cmd;
- error=this->receive_status_packet_v1(&data,&length_v1);
+ this->usb_access->enter();
+ if(version==dyn_version1)
+ {
+ cmd=new unsigned char[length+1];
+ cmd[0]=address%256;
+ for(i=0;i<length;i++)
+ cmd[1+i]=values[i];
+ this->send_instruction_packet_v1(dyn_reg_write,cmd,length+1);
+ delete[] cmd;
+ error=this->receive_status_packet_v1(&data,&length_v1);
+ }
+ else
+ {
+ cmd=new unsigned char[length+2];
+ cmd[0]=address%256;
+ cmd[1]=address/256;
+ for(i=0;i<length;i++)
+ cmd[2+i]=values[i];
+ this->send_instruction_packet_v2(dyn_reg_write,cmd,length+2);
+ delete[] cmd;
+ error=this->receive_status_packet_v2(&data,&length_v2);
+ }
+ if(data!=NULL)
+ delete[] data;
+ this->handle_error(error);
+ this->usb_access->exit();
}
- else
+ catch(CEventTimeoutException &e)
{
- cmd=new unsigned char[length+2];
- cmd[0]=address%256;
- cmd[1]=address/256;
- for(i=0;i<length;i++)
- cmd[2+i]=values[i];
- this->send_instruction_packet_v2(dyn_reg_write,cmd,length+2);
- delete[] cmd;
- error=this->receive_status_packet_v2(&data,&length_v2);
+ this->usb_access->exit();
+ throw e;
+ }
+ catch(CException &e)
+ {
+ this->usb_access->exit();
+ throw e;
}
- if(data!=NULL)
- delete[] data;
- this->handle_error(error);
}
}
@@ -694,30 +756,45 @@ void CDynamixel::write_registers(unsigned short int address, unsigned char *valu
if(address!=(unsigned short int)-1)
{
- if(version==dyn_version1)
+ try
{
- cmd=new unsigned char[length+1];
- cmd[0]=address%256;
- for(i=0;i<length;i++)
- cmd[1+i]=values[i];
- this->send_instruction_packet_v1(dyn_write,cmd,length+1);
- delete[] cmd;
- error=this->receive_status_packet_v1(&data,&length_v1);
+ this->usb_access->enter();
+ if(version==dyn_version1)
+ {
+ cmd=new unsigned char[length+1];
+ cmd[0]=address%256;
+ for(i=0;i<length;i++)
+ cmd[1+i]=values[i];
+ this->send_instruction_packet_v1(dyn_write,cmd,length+1);
+ delete[] cmd;
+ error=this->receive_status_packet_v1(&data,&length_v1);
+ }
+ else
+ {
+ cmd=new unsigned char[length+2];
+ cmd[0]=address%256;
+ cmd[1]=address/256;
+ for(i=0;i<length;i++)
+ cmd[2+i]=values[i];
+ this->send_instruction_packet_v2(dyn_write,cmd,length+2);
+ delete[] cmd;
+ error=this->receive_status_packet_v2(&data,&length_v2);
+ }
+ if(data!=NULL)
+ delete[] data;
+ this->handle_error(error);
+ this->usb_access->exit();
}
- else
+ catch(CEventTimeoutException &e)
{
- cmd=new unsigned char[length+2];
- cmd[0]=address%256;
- cmd[1]=address/256;
- for(i=0;i<length;i++)
- cmd[2+i]=values[i];
- this->send_instruction_packet_v2(dyn_write,cmd,length+2);
- delete[] cmd;
- error=this->receive_status_packet_v2(&data,&length_v2);
+ this->usb_access->exit();
+ throw e;
+ }
+ catch(CException &e)
+ {
+ this->usb_access->exit();
+ throw e;
}
- if(data!=NULL)
- delete[] data;
- this->handle_error(error);
}
}
@@ -728,28 +805,45 @@ void CDynamixel::read_registers(unsigned short int address, unsigned char *value
if(address!=(unsigned short int)-1)
{
- if(version==dyn_version1)
+ try
{
- cmd[0]=address%256;
- cmd[1]=length%256;
- this->send_instruction_packet_v1(dyn_read,cmd,2);
- error=this->receive_status_packet_v1(&data,&length_v1);
+ this->usb_access->enter();
+ if(version==dyn_version1)
+ {
+ cmd[0]=address%256;
+ cmd[1]=length%256;
+ this->send_instruction_packet_v1(dyn_read,cmd,2);
+ error=this->receive_status_packet_v1(&data,&length_v1);
+ }
+ else
+ {
+ cmd[0]=address%256;
+ cmd[1]=address/256;
+ cmd[2]=length%256;
+ cmd[3]=length/256;
+ this->send_instruction_packet_v2(dyn_read,cmd,4);
+ error=this->receive_status_packet_v2(&data,&length_v2);
+ }
+ this->usb_access->exit();
}
- else
+ catch(CEventTimeoutException &e)
{
- cmd[0]=address%256;
- cmd[1]=address/256;
- cmd[2]=length%256;
- cmd[3]=length/256;
- this->send_instruction_packet_v2(dyn_read,cmd,4);
- error=this->receive_status_packet_v2(&data,&length_v2);
+ this->usb_access->exit();
+ throw e;
+ }
+ catch(CException &e)
+ {
+ this->usb_access->exit();
+ throw e;
}
try{
this->handle_error(error);
- for(i=0;i<length;i++)
- values[i]=data[i];
- if(data!=NULL)
+ if(data!=NULL)
+ {
+ for(i=0;i<length;i++)
+ values[i]=data[i];
delete[] data;
+ }
}catch(CException &e){
if(data!=NULL)
delete[] data;
@@ -763,16 +857,31 @@ void CDynamixel::reset(dyn_reset_mode_t mode)
unsigned char *data,error,length_v1,cmd;
unsigned short length_v2;
- if(version==dyn_version1)
+ try
+ {
+ this->usb_access->enter();
+ if(version==dyn_version1)
+ {
+ this->send_instruction_packet_v1(dyn_read,NULL,0);
+ error=this->receive_status_packet_v1(&data,&length_v1);
+ }
+ else
+ {
+ cmd=mode;
+ this->send_instruction_packet_v2(dyn_read,&cmd,1);
+ error=this->receive_status_packet_v2(&data,&length_v2);
+ }
+ this->usb_access->exit();
+ }
+ catch(CEventTimeoutException &e)
{
- this->send_instruction_packet_v1(dyn_read,NULL,0);
- error=this->receive_status_packet_v1(&data,&length_v1);
+ this->usb_access->exit();
+ throw e;
}
- else
+ catch(CException &e)
{
- cmd=mode;
- this->send_instruction_packet_v2(dyn_read,&cmd,1);
- error=this->receive_status_packet_v2(&data,&length_v2);
+ this->usb_access->exit();
+ throw e;
}
try{
this->handle_error(error);
diff --git a/src/dynamixel.h b/src/dynamixel.h
index eebb8cfd0e2b561cce0ccfd9dff91665f03941a6..96c545cae70ec860f23f7876bdacc7ae4d4242c9 100644
--- a/src/dynamixel.h
+++ b/src/dynamixel.h
@@ -78,16 +78,12 @@ class CDynamixel
*
*/
void sync_packet_v2(unsigned char *data,unsigned int length,int *start);
- /**
- * \brief
- *
- */
- void resync(void);
/**
* \brief
*
*/
dyn_version_t version;
+
protected:
/**
* \brief
diff --git a/src/dynamixelserver.cpp b/src/dynamixelserver.cpp
index 1fe70ced63a435cb29ad2bd2cdf11abab72d5c47..7b21203760f3904e5244c82c9c71a4be79e0b9fb 100644
--- a/src/dynamixelserver.cpp
+++ b/src/dynamixelserver.cpp
@@ -96,7 +96,6 @@ void CDynamixelServer::send_instruction_packet_v1(dyn_inst_t inst,unsigned char
if(this->comm_dev!=NULL)
{
try{
- this->dynamixel_access.enter();
if(this->comm_dev->write(packet,length)!=length)
{
/* handle exceptions */
@@ -106,9 +105,7 @@ void CDynamixelServer::send_instruction_packet_v1(dyn_inst_t inst,unsigned char
}
if(packet!=NULL)
delete[] packet;
- this->dynamixel_access.exit();
}catch(CException &e){
- this->dynamixel_access.exit();
throw e;
}
}
@@ -162,7 +159,6 @@ void CDynamixelServer::send_instruction_packet_v2(dyn_inst_t inst,unsigned char
if(this->comm_dev!=NULL)
{
try{
- this->dynamixel_access.enter();
if(this->comm_dev->write(packet,length)!=length)
{
/* handle exceptions */
@@ -172,9 +168,7 @@ void CDynamixelServer::send_instruction_packet_v2(dyn_inst_t inst,unsigned char
}
if(packet!=NULL)
delete[] packet;
- this->dynamixel_access.exit();
}catch(CException &e){
- this->dynamixel_access.exit();
throw e;
}
}
@@ -196,7 +190,6 @@ unsigned char CDynamixelServer::receive_status_packet_v1(unsigned char **data,un
if(this->comm_dev!=NULL)
{
try{
- this->dynamixel_access.enter();
events.push_back(this->comm_dev->get_rx_event_id());
// read up to the length field
do{
@@ -256,13 +249,10 @@ unsigned char CDynamixelServer::receive_status_packet_v1(unsigned char **data,un
*data=NULL;
*len=0;
}
- this->dynamixel_access.exit();
return data_int[start+4];
}catch(CEventTimeoutException &e){
- this->dynamixel_access.exit();
throw e;
}catch(CException &e){
- this->dynamixel_access.exit();
throw e;
}
}
@@ -283,7 +273,6 @@ unsigned char CDynamixelServer::receive_status_packet_v2(unsigned char **data,un
if(this->comm_dev!=NULL)
{
try{
- this->dynamixel_access.enter();
events.push_back(this->comm_dev->get_rx_event_id());
// read up to the length field
do{
@@ -343,13 +332,10 @@ unsigned char CDynamixelServer::receive_status_packet_v2(unsigned char **data,un
*data=NULL;
*len=0;
}
- this->dynamixel_access.exit();
return data_int[start+8];
}catch(CEventTimeoutException &e){
- this->dynamixel_access.exit();
throw e;
}catch(CException &e){
- this->dynamixel_access.exit();
throw e;
}
}
@@ -888,34 +874,63 @@ void CDynamixelServer::free_device(int dev_id,dyn_version_t version)
void CDynamixelServer::action(dyn_version_t version)
{
- if(version==dyn_version1)
- this->send_instruction_packet_v1(dyn_action,NULL,0);
- else
- this->send_instruction_packet_v2(dyn_action,NULL,0);
+ try
+ {
+ this->dynamixel_access.enter();
+ if(version==dyn_version1)
+ this->send_instruction_packet_v1(dyn_action,NULL,0);
+ else
+ this->send_instruction_packet_v2(dyn_action,NULL,0);
+ this->dynamixel_access.exit();
+ }
+ catch(CEventTimeoutException &e)
+ {
+ this->dynamixel_access.exit();
+ throw e;
+ }
+ catch(CException &e)
+ {
+ this->dynamixel_access.exit();
+ throw e;
+ }
}
void CDynamixelServer::ping(int dev_id,int time,dyn_version_t version,unsigned short *model,unsigned char *fw_ver)
{
unsigned char *data,length_v1;
unsigned short length_v2;
-
- if(version==dyn_version1)
+ try
{
- this->send_instruction_packet_v1(dyn_ping,NULL,0,dev_id);
- this->receive_status_packet_v1(&data,&length_v1);
- if(data!=NULL)
- delete[] data;
+ this->dynamixel_access.enter();
+ if(version==dyn_version1)
+ {
+ this->send_instruction_packet_v1(dyn_ping,NULL,0,dev_id);
+ this->receive_status_packet_v1(&data,&length_v1);
+ if(data!=NULL)
+ delete[] data;
+ }
+ else
+ {
+ this->send_instruction_packet_v2(dyn_ping,NULL,0,dev_id);
+ this->receive_status_packet_v2(&data,&length_v2);
+ if(model!=NULL)
+ *model=data[0]+data[1]*256;
+ if(fw_ver!=NULL)
+ *fw_ver=data[2];
+ if(data!=NULL)
+ delete[] data;
+ }
+ this->dynamixel_access.exit();
}
- else
+ catch(CEventTimeoutException &e)
{
- this->send_instruction_packet_v2(dyn_ping,NULL,0,dev_id);
- this->receive_status_packet_v2(&data,&length_v2);
- if(model!=NULL)
- *model=data[0]+data[1]*256;
- if(fw_ver!=NULL)
- *fw_ver=data[2];
- if(data!=NULL)
- delete[] data;
+ this->dynamixel_access.exit();
+ throw e;
+ }
+ catch(CException &e)
+ {
+ this->dynamixel_access.exit();
+ throw e;
}
}
@@ -966,7 +981,22 @@ void CDynamixelServer::write_sync(std::vector<unsigned char>& servo_ids,unsigned
for(j=0;j<data[i].size();j++)
data_int[2+(data[i].size()+1)*i+1+j]=data[i][j];
}
- this->send_instruction_packet_v1(dyn_sync_write,data_int,length);
+ try
+ {
+ this->dynamixel_access.enter();
+ this->send_instruction_packet_v1(dyn_sync_write,data_int,length);
+ this->dynamixel_access.exit();
+ }
+ catch(CEventTimeoutException &e)
+ {
+ this->dynamixel_access.exit();
+ throw e;
+ }
+ catch(CException &e)
+ {
+ this->dynamixel_access.exit();
+ throw e;
+ }
delete[] data_int;
}
else
@@ -983,7 +1013,22 @@ void CDynamixelServer::write_sync(std::vector<unsigned char>& servo_ids,unsigned
for(j=0;j<data[i].size();j++)
data_int[4+(data[i].size()+1)*i+1+j]=data[i][j];
}
- this->send_instruction_packet_v2(dyn_sync_write,data_int,length);
+ try
+ {
+ this->dynamixel_access.enter();
+ this->send_instruction_packet_v2(dyn_sync_write,data_int,length);
+ this->dynamixel_access.exit();
+ }
+ catch(CEventTimeoutException &e)
+ {
+ this->dynamixel_access.exit();
+ throw e;
+ }
+ catch(CException &e)
+ {
+ this->dynamixel_access.exit();
+ throw e;
+ }
delete[] data_int;
}
}catch(CException &e){
@@ -1019,35 +1064,50 @@ void CDynamixelServer::read_sync(std::vector<unsigned char>& servo_ids,unsigned
data_int[3]=length/256;
for(i=0;i<servo_ids.size();i++)
data_int[4+i]=servo_ids[i];
- this->send_instruction_packet_v2(dyn_sync_read,data_int,length_int);
- /* read all the data from all the servos */
- if(data_int!=NULL)
+ try
{
- delete[] data_int;
- data_int=NULL;
+ this->dynamixel_access.enter();
+ this->send_instruction_packet_v2(dyn_sync_read,data_int,length_int);
+ /* read all the data from all the servos */
+ if(data_int!=NULL)
+ {
+ delete[] data_int;
+ data_int=NULL;
+ }
+ data.resize(servo_ids.size());
+ for(i=0;i<servo_ids.size();i++)
+ {
+ try{
+ error=this->receive_status_packet_v2(&data_out,&length_v2);
+ this->handle_error(error);
+ if(data_out!=NULL)
+ {
+ data[i].resize(length_v2);
+ for(j=0;j<length_v2;j++)
+ data[i][j]=data_out[j];
+ delete[] data_out;
+ data_out=NULL;
+ }
+ }catch(CException &e){
+ if(data_out!=NULL)
+ {
+ delete[] data_out;
+ data_out=NULL;
+ }
+ throw e;
+ }
+ }
+ this->dynamixel_access.exit();
}
- data.resize(servo_ids.size());
- for(i=0;i<servo_ids.size();i++)
+ catch(CEventTimeoutException &e)
{
- try{
- error=this->receive_status_packet_v2(&data_out,&length_v2);
- this->handle_error(error);
- if(data_out!=NULL)
- {
- data[i].resize(length_v2);
- for(j=0;j<length_v2;j++)
- data[i][j]=data_out[j];
- delete[] data_out;
- data_out=NULL;
- }
- }catch(CException &e){
- if(data_out!=NULL)
- {
- delete[] data_out;
- data_out=NULL;
- }
- throw e;
- }
+ this->dynamixel_access.exit();
+ throw e;
+ }
+ catch(CException &e)
+ {
+ this->dynamixel_access.exit();
+ throw e;
}
}
else
@@ -1058,35 +1118,50 @@ void CDynamixelServer::read_sync(std::vector<unsigned char>& servo_ids,unsigned
data_int[1]=(unsigned char)length;
for(i=0;i<servo_ids.size();i++)
data_int[2+i]=servo_ids[i];
- this->send_instruction_packet_v1(dyn_sync_read,data_int,length_int);
- /* read all the data from all the servos */
- if(data_int!=NULL)
+ try
{
- delete[] data_int;
- data_int=NULL;
+ this->dynamixel_access.enter();
+ this->send_instruction_packet_v1(dyn_sync_read,data_int,length_int);
+ /* read all the data from all the servos */
+ if(data_int!=NULL)
+ {
+ delete[] data_int;
+ data_int=NULL;
+ }
+ data.resize(servo_ids.size());
+ for(i=0;i<servo_ids.size();i++)
+ {
+ try{
+ error=this->receive_status_packet_v1(&data_out,&length_v1);
+ this->handle_error(error);
+ if(data_out!=NULL)
+ {
+ data[i].resize(length_v1);
+ for(j=0;j<length_v1;j++)
+ data[i][j]=data_out[j];
+ delete[] data_out;
+ data_out=NULL;
+ }
+ }catch(CException &e){
+ if(data_out!=NULL)
+ {
+ delete[] data_out;
+ data_out=NULL;
+ }
+ throw e;
+ }
+ }
+ this->dynamixel_access.exit();
}
- data.resize(servo_ids.size());
- for(i=0;i<servo_ids.size();i++)
+ catch(CEventTimeoutException &e)
{
- try{
- error=this->receive_status_packet_v1(&data_out,&length_v1);
- this->handle_error(error);
- if(data_out!=NULL)
- {
- data[i].resize(length_v1);
- for(j=0;j<length_v1;j++)
- data[i][j]=data_out[j];
- delete[] data_out;
- data_out=NULL;
- }
- }catch(CException &e){
- if(data_out!=NULL)
- {
- delete[] data_out;
- data_out=NULL;
- }
- throw e;
- }
+ this->dynamixel_access.exit();
+ throw e;
+ }
+ catch(CException &e)
+ {
+ this->dynamixel_access.exit();
+ throw e;
}
}
}catch(CException &e){
@@ -1138,7 +1213,22 @@ void CDynamixelServer::write_bulk(std::vector<unsigned char>& servo_ids,std::vec
data_int[offset+5+j]=data[i][j];
offset+=5+data[i].size();
}
- this->send_instruction_packet_v2(dyn_bulk_write,data_int,length);
+ try
+ {
+ this->dynamixel_access.enter();
+ this->send_instruction_packet_v2(dyn_bulk_write,data_int,length);
+ this->dynamixel_access.exit();
+ }
+ catch(CEventTimeoutException &e)
+ {
+ this->dynamixel_access.exit();
+ throw e;
+ }
+ catch(CException &e)
+ {
+ this->dynamixel_access.exit();
+ throw e;
+ }
delete[] data_int;
}
else
@@ -1156,7 +1246,22 @@ void CDynamixelServer::write_bulk(std::vector<unsigned char>& servo_ids,std::vec
data_int[offset+3+j]=data[i][j];
offset+=3+data[i].size();
}
- this->send_instruction_packet_v1(dyn_bulk_write,data_int,length);
+ try
+ {
+ this->dynamixel_access.enter();
+ this->send_instruction_packet_v1(dyn_bulk_write,data_int,length);
+ this->dynamixel_access.exit();
+ }
+ catch(CEventTimeoutException &e)
+ {
+ this->dynamixel_access.exit();
+ throw e;
+ }
+ catch(CException &e)
+ {
+ this->dynamixel_access.exit();
+ throw e;
+ }
delete[] data_int;
}
}catch(CException &e){
@@ -1204,35 +1309,50 @@ void CDynamixelServer::read_bulk(std::vector<unsigned char>& servo_ids,std::vect
data_int[i*5+3]=length[i]%256;
data_int[i*5+4]=length[i]/256;
}
- this->send_instruction_packet_v2(dyn_bulk_read,data_int,length_int);
- /* read all the data from all the servos */
- if(data_int!=NULL)
- {
- delete[] data_int;
- data_int=NULL;
- }
- data.resize(servo_ids.size());
- for(i=0;i<servo_ids.size();i++)
+ try
{
- error=this->receive_status_packet_v2(&data_out,&length_v2);
- try{
- this->handle_error(error);
- if(data_out!=NULL)
- {
- data[i].resize(length_v2);
- for(j=0;j<length_v2;j++)
- data[i][j]=data_out[j];
- delete[] data_out;
- data_out=NULL;
- }
- }catch(CException &e){
- if(data_out!=NULL)
- {
- delete[] data_out;
- data_out=NULL;
+ this->dynamixel_access.enter();
+ this->send_instruction_packet_v2(dyn_bulk_read,data_int,length_int);
+ /* read all the data from all the servos */
+ if(data_int!=NULL)
+ {
+ delete[] data_int;
+ data_int=NULL;
+ }
+ data.resize(servo_ids.size());
+ for(i=0;i<servo_ids.size();i++)
+ {
+ error=this->receive_status_packet_v2(&data_out,&length_v2);
+ try{
+ this->handle_error(error);
+ if(data_out!=NULL)
+ {
+ data[i].resize(length_v2);
+ for(j=0;j<length_v2;j++)
+ data[i][j]=data_out[j];
+ delete[] data_out;
+ data_out=NULL;
+ }
+ }catch(CException &e){
+ if(data_out!=NULL)
+ {
+ delete[] data_out;
+ data_out=NULL;
+ }
+ throw e;
}
- throw e;
}
+ this->dynamixel_access.exit();
+ }
+ catch(CEventTimeoutException &e)
+ {
+ this->dynamixel_access.exit();
+ throw e;
+ }
+ catch(CException &e)
+ {
+ this->dynamixel_access.exit();
+ throw e;
}
}
else
@@ -1245,35 +1365,50 @@ void CDynamixelServer::read_bulk(std::vector<unsigned char>& servo_ids,std::vect
data_int[i*3+1]=(unsigned char)start_addr[i];
data_int[i*3+2]=(unsigned char)length[i];
}
- this->send_instruction_packet_v1(dyn_bulk_read,data_int,length_int);
- /* read all the data from all the servos */
- if(data_int!=NULL)
+ try
{
- delete[] data_int;
- data_int=NULL;
- }
- data.resize(servo_ids.size());
- for(i=0;i<servo_ids.size();i++)
- {
- error=this->receive_status_packet_v1(&data_out,&length_v1);
- try{
- this->handle_error(error);
- if(data_out!=NULL)
- {
- data[i].resize(length_v1);
- for(j=0;j<length_v1;j++)
- data[i][j]=data_out[j];
- delete[] data_out;
- data_out=NULL;
- }
- }catch(CException &e){
- if(data_out!=NULL)
- {
- delete[] data_out;
- data_out=NULL;
+ this->dynamixel_access.enter();
+ this->send_instruction_packet_v1(dyn_bulk_read,data_int,length_int);
+ /* read all the data from all the servos */
+ if(data_int!=NULL)
+ {
+ delete[] data_int;
+ data_int=NULL;
+ }
+ data.resize(servo_ids.size());
+ for(i=0;i<servo_ids.size();i++)
+ {
+ error=this->receive_status_packet_v1(&data_out,&length_v1);
+ try{
+ this->handle_error(error);
+ if(data_out!=NULL)
+ {
+ data[i].resize(length_v1);
+ for(j=0;j<length_v1;j++)
+ data[i][j]=data_out[j];
+ delete[] data_out;
+ data_out=NULL;
+ }
+ }catch(CException &e){
+ if(data_out!=NULL)
+ {
+ delete[] data_out;
+ data_out=NULL;
+ }
+ throw e;
}
- throw e;
}
+ this->dynamixel_access.exit();
+ }
+ catch(CEventTimeoutException &e)
+ {
+ this->dynamixel_access.exit();
+ throw e;
+ }
+ catch(CException &e)
+ {
+ this->dynamixel_access.exit();
+ throw e;
}
}
}catch(CException &e){
diff --git a/src/examples/test_dynamixel_server_no_scan.cpp b/src/examples/test_dynamixel_server_no_scan.cpp
index 4f0ae53f600dffacdad3566e85bc29c520ccf21d..58d3426c99022e295305ec4f7dde3659e637dfbd 100644
--- a/src/examples/test_dynamixel_server_no_scan.cpp
+++ b/src/examples/test_dynamixel_server_no_scan.cpp
@@ -8,10 +8,10 @@ int main(int argc, char *argv[])
std::vector<int> devices;
CDynamixel *dyn_motor;
int num_buses=0;
- int baudrate; //57600 or 100000
+ int baudrate; //57600 or 1000000
bool found=false;
int first=0;
- int last=5;
+ int last=2;
if(argc==3)
{
first = atoi(argv[1]);
@@ -28,7 +28,7 @@ int main(int argc, char *argv[])
break;
- baudrate = 57600;
+ baudrate = 1000000;
dyn_server->config_bus(b,baudrate);
std::string serial = dyn_server->get_bus_serial();
std::cout << "For bus with id " << b << ", serial "<< serial << std::endl;
@@ -38,6 +38,7 @@ int main(int argc, char *argv[])
try
{
dyn_motor=dyn_server->get_device(i);
+ dyn_motor->reset();
int id = (int)dyn_motor->get_id();
std::cout << "- FOUND device "<< id << " with baudrate " << baudrate << " on bus with id " << b << " and serial "<< serial << std::endl;
dyn_server->free_device(i);