diff --git a/src/dynamixel.cpp b/src/dynamixel.cpp index bc82acfc918127c9142dc2a2f99bc7c7769ece55..897fd30fd170045b22fcc746411ba21dbdfb5e10 100644 --- a/src/dynamixel.cpp +++ b/src/dynamixel.cpp @@ -124,7 +124,7 @@ unsigned char CDynamixel::receive_status_packet_v1(unsigned char **data,unsigned do{ if((num=this->usb_dev->get_num_data())==0) { - this->event_server->wait_all(events,20); + this->event_server->wait_all(events,50); num=this->usb_dev->get_num_data(); } if((read+num)>1024) @@ -145,7 +145,7 @@ unsigned char CDynamixel::receive_status_packet_v1(unsigned char **data,unsigned { if((num=this->usb_dev->get_num_data())==0) { - this->event_server->wait_all(events,20); + this->event_server->wait_all(events,50); num=this->usb_dev->get_num_data(); } if((read-start+num)>length) @@ -209,7 +209,7 @@ unsigned char CDynamixel::receive_status_packet_v2(unsigned char **data,unsigned do{ if((num=this->usb_dev->get_num_data())==0) { - this->event_server->wait_all(events,20); + this->event_server->wait_all(events,50); num=this->usb_dev->get_num_data(); } if((read+num)>256) @@ -230,7 +230,7 @@ unsigned char CDynamixel::receive_status_packet_v2(unsigned char **data,unsigned { if((num=this->usb_dev->get_num_data())==0) { - this->event_server->wait_all(events,20); + this->event_server->wait_all(events,50); num=this->usb_dev->get_num_data(); } if((read-start+num)>256) @@ -465,31 +465,34 @@ void CDynamixel::read_byte_register(unsigned short int address,unsigned char *va unsigned char *data,error,length_v1,cmd[4]; unsigned short length_v2; - 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 + if(address!=(unsigned short int)-1) { - 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); - } - try{ - this->handle_error(error); - *value=data[0]; - if(data!=NULL) - delete[] data; - }catch(CException &e){ - if(data!=NULL) - delete[] data; - throw e; + 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); + } + try{ + this->handle_error(error); + *value=data[0]; + if(data!=NULL) + delete[] data; + }catch(CException &e){ + if(data!=NULL) + delete[] data; + throw e; + } } } @@ -498,31 +501,34 @@ void CDynamixel::read_word_register(unsigned short int address,unsigned short in unsigned char *data,error,length_v1,cmd[4]; unsigned short length_v2; - if(version==dyn_version1) + if(address!=(unsigned short int)-1) { - 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); - } - try{ - this->handle_error(error); - *value=data[0]+data[1]*256; - if(data!=NULL) - delete[] data; - }catch(CException &e){ - if(data!=NULL) - delete[] data; - throw e; + 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); + } + try{ + this->handle_error(error); + *value=data[0]+data[1]*256; + if(data!=NULL) + delete[] data; + }catch(CException &e){ + if(data!=NULL) + delete[] data; + throw e; + } } } @@ -531,24 +537,27 @@ void CDynamixel::write_byte_register(unsigned short int address, unsigned char v unsigned char *data,error,length_v1,cmd[3]; unsigned short length_v2; - 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 + if(address!=(unsigned short int)-1) { - 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(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); } - if(data!=NULL) - delete[] data; - this->handle_error(error); } void CDynamixel::write_word_register(unsigned short int address, unsigned short int value) @@ -556,26 +565,29 @@ void CDynamixel::write_word_register(unsigned short int address, unsigned short unsigned char *data,error,length_v1,cmd[4]; unsigned short length_v2; - if(version==dyn_version1) + if(address!=(unsigned short int)-1) { - 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(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); } - if(data!=NULL) - delete[] data; - this->handle_error(error); } void CDynamixel::registered_byte_write(unsigned short int address, unsigned char value) @@ -583,51 +595,57 @@ void CDynamixel::registered_byte_write(unsigned short int address, unsigned char unsigned char *data,error,length_v1,cmd[3]; unsigned short length_v2; - if(version==dyn_version1) + if(address!=(unsigned short int)-1) { - 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(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); } - if(data!=NULL) - delete[] data; - this->handle_error(error); -} +} void CDynamixel::registered_word_write(unsigned short int address, unsigned short int value) { unsigned char *data,error,length_v1,cmd[4]; unsigned short length_v2; - if(version==dyn_version1) + if(address==(unsigned short int)-1) { - 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(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); } - if(data!=NULL) - delete[] data; - this->handle_error(error); } void CDynamixel::registered_write(unsigned short int address, unsigned char *values,unsigned int length) @@ -635,30 +653,33 @@ void CDynamixel::registered_write(unsigned short int address, unsigned char *val unsigned char *data,error,length_v1,*cmd; unsigned short length_v2,i; - 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 + if(address!=(unsigned short int)-1) { - 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(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); } - if(data!=NULL) - delete[] data; - this->handle_error(error); } void CDynamixel::write_registers(unsigned short int address, unsigned char *values, unsigned int length) @@ -666,30 +687,33 @@ void CDynamixel::write_registers(unsigned short int address, unsigned char *valu unsigned char *data,error,length_v1,*cmd; unsigned short length_v2,i; - 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 + if(address!=(unsigned short int)-1) { - 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(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); } - if(data!=NULL) - delete[] data; - this->handle_error(error); } void CDynamixel::read_registers(unsigned short int address, unsigned char *values, unsigned int length) @@ -697,32 +721,35 @@ void CDynamixel::read_registers(unsigned short int address, unsigned char *value unsigned char *data,error,length_v1,cmd[4]; unsigned short length_v2,i; - 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 + if(address!=(unsigned short int)-1) { - 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); - } - try{ - this->handle_error(error); - for(i=0;i<length;i++) - values[i]=data[i]; - if(data!=NULL) - delete[] data; - }catch(CException &e){ - if(data!=NULL) - delete[] data; - throw e; + 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); + } + try{ + this->handle_error(error); + for(i=0;i<length;i++) + values[i]=data[i]; + if(data!=NULL) + delete[] data; + }catch(CException &e){ + if(data!=NULL) + delete[] data; + throw e; + } } }