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

Added exception handling in some functions. 

Re-indented some parts of the code.
parent b32ef64d
No related branches found
No related tags found
No related merge requests found
Hide whitespace changes
Inline Side-by-side
src/dynamixel_slave.cpp
+ 107
− 92
View file @ 410a20a0
...@@ -56,99 +56,103 @@ void *CDynamixelSlave::process_packets_thread(void *params) ...@@ -56,99 +56,103 @@ void *CDynamixelSlave::process_packets_thread(void *params)
end=true; end=true;
else else
{ {
// process the incomming data try{
slave->comm_access.enter(); // process the incomming data
num=slave->comm_dev->get_num_data(); slave->comm_access.enter();
if(slave->comm_dev->read(data,num)!=num) num=slave->comm_dev->get_num_data();
{ if(slave->comm_dev->read(data,num)!=num)
slave->comm_access.exit(); {
std::cout << "Error while reading the communication device" << std::endl; slave->comm_access.exit();
} std::cout << "Error while reading the communication device" << std::endl;
else }
{ else
slave->comm_access.exit(); {
for(i=0;i<num;i++) slave->comm_access.exit();
{ for(i=0;i<num;i++)
if(!data_phase)
{ {
switch(num_bytes) if(!data_phase)
{ {
case 0: if(data[i]==0xFF) switch(num_bytes)
{ {
packet[num_bytes]=data[i]; case 0: if(data[i]==0xFF)
num_bytes++; {
} packet[num_bytes]=data[i];
break; num_bytes++;
case 1: if(data[i]==0xFF) }
{ break;
packet[num_bytes]=data[i]; case 1: if(data[i]==0xFF)
num_bytes++; {
} packet[num_bytes]=data[i];
else num_bytes++;
num_bytes--; }
break; else
case 2: if(data[i]==0xFD)// version 2 header num_bytes--;
{ break;
if(slave->dyn_ver==dyn_version2)// the module is configured for version 2 case 2: if(data[i]==0xFD)// version 2 header
{ {
packet[num_bytes]=data[i]; if(slave->dyn_ver==dyn_version2)// the module is configured for version 2
num_bytes++; {
} packet[num_bytes]=data[i];
else num_bytes++;
num_bytes=0;// ignore packet and restart synchronization }
} else
else if(data[i]!=0xFF) num_bytes=0;// ignore packet and restart synchronization
{ }
packet[num_bytes]=data[i]; else if(data[i]!=0xFF)
num_bytes++; {
} packet[num_bytes]=data[i];
break; num_bytes++;
case 3: packet[num_bytes]=data[i]; }
if(slave->dyn_ver==dyn_version1) break;
{ case 3: packet[num_bytes]=data[i];
length=data[i]; if(slave->dyn_ver==dyn_version1)
num_bytes++; {
length=data[i];
num_bytes++;
/* read packet_data */
data_phase=true;
}
else
num_bytes++;
break;
case 4: packet[num_bytes]=data[i];
num_bytes++;
break;
case 5: packet[num_bytes]=data[i];
num_bytes++;
length=data[i];
break;
case 6: packet[num_bytes]=data[i];
num_bytes++;
length+=(data[i]<<8);
/* read packet_data */ /* read packet_data */
data_phase=true; data_phase=true;
} break;
else default: break;
num_bytes++; }
break; }
case 4: packet[num_bytes]=data[i]; else// data phase
num_bytes++;
break;
case 5: packet[num_bytes]=data[i];
num_bytes++;
length=data[i];
break;
case 6: packet[num_bytes]=data[i];
num_bytes++;
length+=(data[i]<<8);
/* read packet_data */
data_phase=true;
break;
default: break;
}
}
else// data phase
{
packet[num_bytes]=data[i];
num_bytes++;
length--;
if(length==0)
{ {
data_phase=false; packet[num_bytes]=data[i];
memcpy(new_data,packet,num_bytes); num_bytes++;
slave->packets.push_back(new_data); length--;
if(slave->dyn_ver==dyn_version1) if(length==0)
slave->dynamixel_loop_v1(); {
else data_phase=false;
slave->dynamixel_loop_v2(); memcpy(new_data,packet,num_bytes);
//slave->event_server->set_event(slave->new_packet_available_event_id); slave->packets.push_back(new_data);
num_bytes=0; if(slave->dyn_ver==dyn_version1)
slave->dynamixel_loop_v1();
else
slave->dynamixel_loop_v2();
//slave->event_server->set_event(slave->new_packet_available_event_id);
num_bytes=0;
}
} }
} }
} }
}catch(CException &e){
} }
} }
} }
...@@ -172,11 +176,14 @@ void *CDynamixelSlave::dynamixel_loop_thread(void *params) ...@@ -172,11 +176,14 @@ void *CDynamixelSlave::dynamixel_loop_thread(void *params)
end=true; end=true;
else else
{ {
// process the packets try{
if(slave->dyn_ver==dyn_version1) // process the packets
slave->dynamixel_loop_v1(); if(slave->dyn_ver==dyn_version1)
else slave->dynamixel_loop_v1();
slave->dynamixel_loop_v2(); else
slave->dynamixel_loop_v2();
}catch(CException &e){
}
} }
} }
...@@ -844,10 +851,18 @@ void CDynamixelSlave::get_return_level(return_level_t level) ...@@ -844,10 +851,18 @@ void CDynamixelSlave::get_return_level(return_level_t level)
CDynamixelSlave::~CDynamixelSlave() CDynamixelSlave::~CDynamixelSlave()
{ {
this->event_server->set_event(this->finish_thread_event_id);
this->event_server->set_event(this->finish_thread_event_id);
if(this->thread_server->get_thread_state(this->process_packets_thread_id)==starting ||
this->thread_server->get_thread_state(this->process_packets_thread_id)==active)
this->thread_server->end_thread(this->process_packets_thread_id);
this->thread_server->detach_thread(this->process_packets_thread_id); this->thread_server->detach_thread(this->process_packets_thread_id);
this->thread_server->delete_thread(this->process_packets_thread_id); this->thread_server->delete_thread(this->process_packets_thread_id);
this->process_packets_thread_id=""; this->process_packets_thread_id="";
if(this->thread_server->get_thread_state(this->dynamixel_loop_thread_id)==starting ||
this->thread_server->get_thread_state(this->dynamixel_loop_thread_id)==active)
this->thread_server->end_thread(this->dynamixel_loop_thread_id);
this->thread_server->detach_thread(this->dynamixel_loop_thread_id); this->thread_server->detach_thread(this->dynamixel_loop_thread_id);
this->thread_server->delete_thread(this->dynamixel_loop_thread_id); this->thread_server->delete_thread(this->dynamixel_loop_thread_id);
this->dynamixel_loop_thread_id=""; this->dynamixel_loop_thread_id="";
......
src/dynamixel_slave_serial.cpp
+ 9
− 1
View file @ 410a20a0
...@@ -51,5 +51,13 @@ std::string CDynamixelSlaveSerial::get_serial_device(void) ...@@ -51,5 +51,13 @@ std::string CDynamixelSlaveSerial::get_serial_device(void)
CDynamixelSlaveSerial::~CDynamixelSlaveSerial() CDynamixelSlaveSerial::~CDynamixelSlaveSerial()
{ {
this->stop(); try{
this->stop();
if(this->comm_dev!=NULL)
{
delete this->comm_dev;
this->comm_dev=NULL;
}
}catch(CException &e){
}
} }
0% or .
You are about to add 0 people to the discussion. Proceed with caution.
Finish editing this message first!
Please register or to comment