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)
end=true;
else
{
// process the incomming data
slave->comm_access.enter();
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;
}
else
{
slave->comm_access.exit();
for(i=0;i<num;i++)
{
if(!data_phase)
try{
// process the incomming data
slave->comm_access.enter();
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;
}
else
{
slave->comm_access.exit();
for(i=0;i<num;i++)
{
switch(num_bytes)
{
case 0: if(data[i]==0xFF)
{
packet[num_bytes]=data[i];
num_bytes++;
}
break;
case 1: if(data[i]==0xFF)
{
packet[num_bytes]=data[i];
num_bytes++;
}
else
num_bytes--;
break;
case 2: if(data[i]==0xFD)// version 2 header
{
if(slave->dyn_ver==dyn_version2)// the module is configured for version 2
{
packet[num_bytes]=data[i];
num_bytes++;
}
else
num_bytes=0;// ignore packet and restart synchronization
}
else if(data[i]!=0xFF)
{
packet[num_bytes]=data[i];
num_bytes++;
}
break;
case 3: packet[num_bytes]=data[i];
if(slave->dyn_ver==dyn_version1)
{
length=data[i];
num_bytes++;
if(!data_phase)
{
switch(num_bytes)
{
case 0: if(data[i]==0xFF)
{
packet[num_bytes]=data[i];
num_bytes++;
}
break;
case 1: if(data[i]==0xFF)
{
packet[num_bytes]=data[i];
num_bytes++;
}
else
num_bytes--;
break;
case 2: if(data[i]==0xFD)// version 2 header
{
if(slave->dyn_ver==dyn_version2)// the module is configured for version 2
{
packet[num_bytes]=data[i];
num_bytes++;
}
else
num_bytes=0;// ignore packet and restart synchronization
}
else if(data[i]!=0xFF)
{
packet[num_bytes]=data[i];
num_bytes++;
}
break;
case 3: packet[num_bytes]=data[i];
if(slave->dyn_ver==dyn_version1)
{
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 */
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 */
data_phase=true;
break;
default: break;
}
}
else// data phase
{
packet[num_bytes]=data[i];
num_bytes++;
length--;
if(length==0)
break;
default: break;
}
}
else// data phase
{
data_phase=false;
memcpy(new_data,packet,num_bytes);
slave->packets.push_back(new_data);
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;
packet[num_bytes]=data[i];
num_bytes++;
length--;
if(length==0)
{
data_phase=false;
memcpy(new_data,packet,num_bytes);
slave->packets.push_back(new_data);
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)
end=true;
else
{
// process the packets
if(slave->dyn_ver==dyn_version1)
slave->dynamixel_loop_v1();
else
slave->dynamixel_loop_v2();
try{
// process the packets
if(slave->dyn_ver==dyn_version1)
slave->dynamixel_loop_v1();
else
slave->dynamixel_loop_v2();
}catch(CException &e){
}
}
}
......@@ -844,10 +851,18 @@ void CDynamixelSlave::get_return_level(return_level_t level)
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->delete_thread(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->delete_thread(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)
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