diff --git a/src/dynamixel.cpp b/src/dynamixel.cpp index 897fd30fd170045b22fcc746411ba21dbdfb5e10..35b32428449dc30d170d667ae39ac671778a01c2 100644 --- a/src/dynamixel.cpp +++ b/src/dynamixel.cpp @@ -184,6 +184,9 @@ unsigned char CDynamixel::receive_status_packet_v1(unsigned char **data,unsigned }catch(CEventTimeoutException &e){ this->usb_access->exit(); throw e; + }catch(CException &e){ + this->usb_access->exit(); + throw e; } } else @@ -269,6 +272,9 @@ unsigned char CDynamixel::receive_status_packet_v2(unsigned char **data,unsigned }catch(CEventTimeoutException &e){ this->usb_access->exit(); throw e; + }catch(CException &e){ + this->usb_access->exit(); + throw e; } } else diff --git a/src/dynamixelserver.cpp b/src/dynamixelserver.cpp index 272bf31e269a273a27d802df5afb2aac96d60340..9accb24669575a02cb80b13f56fabecf96832282 100644 --- a/src/dynamixelserver.cpp +++ b/src/dynamixelserver.cpp @@ -116,18 +116,22 @@ void CDynamixelServer::send_instruction_packet_v1(dyn_inst_t inst,unsigned char packet[length-1]=this->compute_checksum_v1(packet,length); if(this->comm_dev!=NULL) { - this->dynamixel_access.enter(); - if(this->comm_dev->write(packet,length)!=length) - { - /* handle exceptions */ + try{ + this->dynamixel_access.enter(); + if(this->comm_dev->write(packet,length)!=length) + { + /* handle exceptions */ + if(packet!=NULL) + delete[] packet; + throw CDynamixelServerException(_HERE_,"Unexpected error while writing to the communication device"); + } if(packet!=NULL) delete[] packet; this->dynamixel_access.exit(); - throw CDynamixelServerException(_HERE_,"Unexpected error while writing to the communication device"); + }catch(CException &e){ + this->dynamixel_access.exit(); + throw e; } - if(packet!=NULL) - delete[] packet; - this->dynamixel_access.exit(); } else { @@ -177,19 +181,23 @@ void CDynamixelServer::send_instruction_packet_v2(dyn_inst_t inst,unsigned char packet[length-2]=crc%256; packet[length-1]=crc/256; if(this->comm_dev!=NULL) - { - this->dynamixel_access.enter(); - if(this->comm_dev->write(packet,length)!=length) - { - /* handle exceptions */ + { + try{ + this->dynamixel_access.enter(); + if(this->comm_dev->write(packet,length)!=length) + { + /* handle exceptions */ + if(packet!=NULL) + delete[] packet; + throw CDynamixelServerException(_HERE_,"Unexpected error while writing to the communication device"); + } if(packet!=NULL) delete[] packet; this->dynamixel_access.exit(); - throw CDynamixelServerException(_HERE_,"Unexpected error while writing to the communication device"); + }catch(CException &e){ + this->dynamixel_access.exit(); + throw e; } - if(packet!=NULL) - delete[] packet; - this->dynamixel_access.exit(); } else { @@ -253,7 +261,6 @@ unsigned char CDynamixelServer::receive_status_packet_v1(unsigned char **data,un // check the checksum if(this->compute_checksum_v1(&data_int[start],length)!=0x00) { - this->dynamixel_access.exit(); /* handle exceptions */ throw CDynamixelServerException(_HERE_,"Invalid Checksum"); } @@ -275,6 +282,9 @@ unsigned char CDynamixelServer::receive_status_packet_v1(unsigned char **data,un }catch(CEventTimeoutException &e){ this->dynamixel_access.exit(); throw e; + }catch(CException &e){ + this->dynamixel_access.exit(); + throw e; } } else @@ -339,7 +349,6 @@ unsigned char CDynamixelServer::receive_status_packet_v2(unsigned char **data,un 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]) { - this->dynamixel_access.exit(); /* handle exceptions */ throw CDynamixelServerException(_HERE_,"Invalid Checksum"); } @@ -360,6 +369,9 @@ unsigned char CDynamixelServer::receive_status_packet_v2(unsigned char **data,un }catch(CEventTimeoutException &e){ this->dynamixel_access.exit(); throw e; + }catch(CException &e){ + this->dynamixel_access.exit(); + throw e; } } else @@ -498,7 +510,6 @@ void CDynamixelServer::handle_error(unsigned char error) error_msg+="\nOverload"; if(error&0x40) error_msg+="\nInvalid instruction"; - std::cout << error_msg << std::endl; throw CDynamixelAlarmException(_HERE_,error_msg,0xFE,error); } } @@ -1134,26 +1145,94 @@ void CDynamixelServer::action(dyn_version_t version) 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 char *data,length_v1,dummy; + std::list<std::string> events; unsigned short length_v2; + int num_retries=5,num,i; + bool done=false; 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; + events.push_back(this->comm_dev->get_rx_event_id()); + while(!done) + { + 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; + done=true; + }catch(CEventTimeoutException &e){ + this->dynamixel_access.enter(); + for(i=0;i<32;i++) + { + if((num=this->comm_dev->get_num_data())==0) + { + try{ + this->comm_dev->write(&dummy,1); + this->event_server->wait_all(events,20); + }catch(CEventTimeoutException &e){ + } + } + else + break; + } + this->dynamixel_access.exit(); + if(num>0) + { + this->receive_status_packet_v1(&data,&length_v1); + if(data!=NULL) + delete[] data; + usleep(100000); + } + num_retries--; + if(num_retries==0) + throw e; + } + } } 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; + events.push_back(this->comm_dev->get_rx_event_id()); + while(!done) + { + try{ + 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; + }catch(CEventTimeoutException &e){ + this->dynamixel_access.enter(); + for(i=0;i<32;i++) + { + if((num=this->comm_dev->get_num_data())==0) + { + try{ + this->comm_dev->write(&dummy,1); + this->event_server->wait_all(events,20); + }catch(CEventTimeoutException &e){ + } + } + else + break; + } + this->dynamixel_access.exit(); + if(num>0) + { + this->receive_status_packet_v1(&data,&length_v1); + if(data!=NULL) + delete[] data; + usleep(100000); + } + num_retries--; + if(num_retries==0) + throw e; + } + } } }