Skip to content
Snippets Groups Projects
Commit 05a04877 authored by Sergi Hernandez's avatar Sergi Hernandez
Browse files

Solved a bug when unexpected data is received. Possible simultaneous read and write operations?

parent c2e0814f
No related branches found
No related tags found
No related merge requests found
......@@ -140,12 +140,9 @@ void CDynamixel::read_byte_register(unsigned char address,unsigned char *value)
try{
while(read<read_length)
{
this->event_server->wait_all(events,2000);
this->event_server->wait_all(events,100);
num=this->usb_dev->get_num_data();
if(num>(read_length-read))
this->usb_dev->read(&data[read],read_length-read);
else
this->usb_dev->read(&data[read],num);
this->usb_dev->read(&data[read],num);
read+=num;
if(read>5 && data[4]!=0x00)
read_length=6;
......@@ -158,7 +155,10 @@ void CDynamixel::read_byte_register(unsigned char address,unsigned char *value)
if(it==NUM_RETRIES)
throw CDynamixelSyncException("Invalid packet checksum");
else
{
read=0;
read_length=7;
}
}
else
{
......@@ -231,12 +231,9 @@ void CDynamixel::read_word_register(unsigned char address,unsigned short int *va
try{
while(read<read_length)
{
this->event_server->wait_all(events,2000);
this->event_server->wait_all(events,100);
num=this->usb_dev->get_num_data();
if(num>(read_length-read))
this->usb_dev->read(&data[read],read_length-read);
else
this->usb_dev->read(&data[read],num);
this->usb_dev->read(&data[read],num);
read+=num;
if(read>5 && data[4]!=0x00)
read_length=6;
......@@ -249,7 +246,10 @@ void CDynamixel::read_word_register(unsigned char address,unsigned short int *va
if(it==NUM_RETRIES)
throw CDynamixelSyncException("Invalid packet checksum");
else
{
read=0;
read_length=8;
}
}
else
{
......@@ -322,12 +322,9 @@ void CDynamixel::write_byte_register(unsigned char address, unsigned char data)
try{
while(read<6)
{
this->event_server->wait_all(events,1000);
this->event_server->wait_all(events,100);
num=this->usb_dev->get_num_data();
if(num>(6-read))
this->usb_dev->read(&answer[read],6-read);
else
this->usb_dev->read(&answer[read],num);
this->usb_dev->read(&answer[read],num);
read+=num;
}
if(this->compute_checksum(answer,6)!=0)
......@@ -346,7 +343,6 @@ void CDynamixel::write_byte_register(unsigned char address, unsigned char data)
this->usb_access->exit();
}
}catch(CEventTimeoutException &e){
std::cout << "Write byte: num bytes read " << read << std::endl;
this->resync();
this->usb_access->exit();
}
......@@ -411,12 +407,9 @@ void CDynamixel::write_word_register(unsigned char address, unsigned short int d
try{
while(read<6)
{
this->event_server->wait_all(events,1000);
this->event_server->wait_all(events,100);
num=this->usb_dev->get_num_data();
if(num>(6-read))
this->usb_dev->read(&answer[read],6-read);
else
this->usb_dev->read(&answer[read],num);
this->usb_dev->read(&answer[read],num);
read+=num;
}
if(this->compute_checksum(answer,6)!=0)
......@@ -435,7 +428,6 @@ void CDynamixel::write_word_register(unsigned char address, unsigned short int d
this->usb_access->exit();
}
}catch(CEventTimeoutException &e){
std::cout << "Write word: num bytes read " << read << std::endl;
this->resync();
this->usb_access->exit();
}
......@@ -499,12 +491,9 @@ void CDynamixel::registered_byte_write(unsigned char address, unsigned char data
try{
while(read<6)
{
this->event_server->wait_all(events,1000);
this->event_server->wait_all(events,100);
num=this->usb_dev->get_num_data();
if(num>(6-read))
this->usb_dev->read(&answer[read],6-read);
else
this->usb_dev->read(&answer[read],num);
this->usb_dev->read(&answer[read],num);
read+=num;
}
if(this->compute_checksum(answer,6)!=0)
......@@ -523,7 +512,6 @@ void CDynamixel::registered_byte_write(unsigned char address, unsigned char data
this->usb_access->exit();
}
}catch(CEventTimeoutException &e){
std::cout << "Registered byte write: num bytes read " << read << std::endl;
this->resync();
this->usb_access->exit();
}
......@@ -588,12 +576,9 @@ void CDynamixel::registered_word_write(unsigned char address, unsigned short int
try{
while(read<6)
{
this->event_server->wait_all(events,1000);
this->event_server->wait_all(events,100);
num=this->usb_dev->get_num_data();
if(num>(6-read))
this->usb_dev->read(&answer[read],6-read);
else
this->usb_dev->read(&answer[read],num);
this->usb_dev->read(&answer[read],num);
read+=num;
}
if(this->compute_checksum(answer,6)!=0)
......@@ -612,7 +597,6 @@ void CDynamixel::registered_word_write(unsigned char address, unsigned short int
this->usb_access->exit();
}
}catch(CEventTimeoutException &e){
std::cout << "Registered word write: num bytes read " << read << std::endl;
this->resync();
this->usb_access->exit();
}
......@@ -671,7 +655,7 @@ void CDynamixel::reset(void)
try{
while(read<6)
{
this->event_server->wait_all(events,1000);
this->event_server->wait_all(events,100);
num=this->usb_dev->get_num_data();
if(num>(6-read))
this->usb_dev->read(&data[read],6-read);
......
0% Loading or .
You are about to add 0 people to the discussion. Proceed with caution.
Finish editing this message first!
Please register or to comment