diff --git a/src/dynamixel.cpp b/src/dynamixel.cpp index 7ddd9471fcd0fa78e48fc103d82ffac8e85aba88..61df2a885df3aec397a17fc06206a2be73be2f39 100644 --- a/src/dynamixel.cpp +++ b/src/dynamixel.cpp @@ -290,7 +290,6 @@ void CDynamixel::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,this->node_address,error); } } diff --git a/src/dynamixelserver.cpp b/src/dynamixelserver.cpp index 822a9c06b33e73788476d795a7b02d5e71341fca..c48e6a8e44da052be9d987322ee1f4589f7b14d4 100644 --- a/src/dynamixelserver.cpp +++ b/src/dynamixelserver.cpp @@ -194,7 +194,9 @@ unsigned char CDynamixelServer::receive_status_packet_v1(unsigned char **data,un do{ if((num=this->comm_dev->get_num_data())==0) { + this->dynamixel_access.exit(); this->event_server->wait_all(events,20); + this->dynamixel_access.enter(); num=this->comm_dev->get_num_data(); } if((read+num)>1024) @@ -215,7 +217,9 @@ unsigned char CDynamixelServer::receive_status_packet_v1(unsigned char **data,un { if((num=this->comm_dev->get_num_data())==0) { + this->dynamixel_access.exit(); this->event_server->wait_all(events,20); + this->dynamixel_access.enter(); num=this->comm_dev->get_num_data(); } if((read-start+num)>length) @@ -277,7 +281,9 @@ unsigned char CDynamixelServer::receive_status_packet_v2(unsigned char **data,un do{ if((num=this->comm_dev->get_num_data())==0) { + this->dynamixel_access.exit(); this->event_server->wait_all(events,20); + this->dynamixel_access.enter(); num=this->comm_dev->get_num_data(); } if((read+num)>1024) @@ -291,14 +297,16 @@ unsigned char CDynamixelServer::receive_status_packet_v2(unsigned char **data,un read+=num; } this->sync_packet_v2(data_int,read,&start); - }while(read<7); + }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=this->comm_dev->get_num_data())==0) { + this->dynamixel_access.exit(); this->event_server->wait_all(events,20); + this->dynamixel_access.enter(); num=this->comm_dev->get_num_data(); } if((read-start+num)>length)