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

Solved a bug in the bulk read operation for version 1 protocol. 

Removed the thread to process the dynamixel loops (it is already done in the data thread).
Added a new signal to notify slave answer to other slaves (simulate half duplex communication).
parent 62003061
No related branches found
No related tags found
No related merge requests found
Hide whitespace changes
Inline Side-by-side
src/dynamixel_slave.cpp
+ 99
− 132
View file @ ab197094
......@@ -6,18 +6,15 @@
CDynamixelSlave::CDynamixelSlave(const std::string& cont_id,dyn_version_t dyn_ver)
{
this->event_server=CEventServer::instance();
this->new_packet_available_event_id=cont_id + "new_packet_event_id";
this->event_server->create_event(this->new_packet_available_event_id);
this->finish_thread_event_id=cont_id + "finish_thread_event_id";
this->event_server->create_event(this->finish_thread_event_id);
this->slave_answer_event_id=cont_id + "slave_answer_event_id";
this->event_server->create_event(this->slave_answer_event_id);
this->thread_server=CThreadServer::instance();
this->process_packets_thread_id=cont_id + "process_packets_thread_id";
this->thread_server->create_thread(this->process_packets_thread_id);
this->thread_server->attach_thread(this->process_packets_thread_id,this->process_packets_thread,this);
this->dynamixel_loop_thread_id=cont_id + "dynamixel_loop_thread_id";
this->thread_server->create_thread(this->dynamixel_loop_thread_id);
this->thread_server->attach_thread(this->dynamixel_loop_thread_id,this->dynamixel_loop_thread,this);
this->return_delay=0;
this->return_level=return_all;
......@@ -30,8 +27,8 @@ void *CDynamixelSlave::process_packets_thread(void *params)
{
CDynamixelSlave *slave=(CDynamixelSlave *)params;
std::list<std::string> events;
int event_index,num,i;
unsigned char data[64*1024],new_data[64*1024];
int event_index,num,i,read;
unsigned char data[1024];
bool end=false;
static bool data_phase=false;
......@@ -48,6 +45,7 @@ void *CDynamixelSlave::process_packets_thread(void *params)
}
end=false;
events.push_back(slave->finish_thread_event_id);
events.push_back(slave->slave_answer_event_id);
events.push_back(slave->comm_dev->get_rx_event_id());
while(!end)
{
......@@ -58,99 +56,109 @@ void *CDynamixelSlave::process_packets_thread(void *params)
{
try{
// process the incomming data
slave->comm_access.enter();
num=slave->comm_dev->get_num_data();
if(slave->comm_dev->read(data,num)!=num)
if(event_index==2)
{
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++)
slave->comm_access.enter();
num=slave->comm_dev->get_num_data();
read=slave->comm_dev->read(data,num);
if(read!=num)
{
if(!data_phase)
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)
if(!data_phase)
{
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
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=0;// ignore packet and restart synchronization
}
else if(data[i]!=0xFF)
{
packet[num_bytes]=data[i];
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++;
}
break;
case 3: packet[num_bytes]=data[i];
if(slave->dyn_ver==dyn_version1)
{
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;
break;
default: break;
}
}
}
else// data phase
{
packet[num_bytes]=data[i];
num_bytes++;
length--;
if(length==0)
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(slave->new_packet,packet,num_bytes);
if(slave->dyn_ver==dyn_version1)
slave->dynamixel_loop_v1();
else
slave->dynamixel_loop_v2();
num_bytes=0;
}
}
}
}
}
else if(event_index==1)
{
memcpy(slave->new_packet,slave->slave_answer,slave->slave_answer_length);
if(slave->dyn_ver==dyn_version1)
slave->dynamixel_loop_v1();
else
slave->dynamixel_loop_v2();
}
}catch(CException &e){
}
......@@ -160,36 +168,6 @@ void *CDynamixelSlave::process_packets_thread(void *params)
pthread_exit(NULL);
}
void *CDynamixelSlave::dynamixel_loop_thread(void *params)
{
CDynamixelSlave *slave=(CDynamixelSlave *)params;
std::list<std::string> events;
int event_index;
bool end=false;
events.push_back(slave->finish_thread_event_id);
events.push_back(slave->new_packet_available_event_id);
while(!end)
{
event_index=slave->event_server->wait_first(events);
if(event_index==0)
end=true;
else
{
try{
// process the packets
if(slave->dyn_ver==dyn_version1)
slave->dynamixel_loop_v1();
else
slave->dynamixel_loop_v2();
}catch(CException &e){
}
}
}
pthread_exit(NULL);
}
void CDynamixelSlave::handle_error(unsigned char error)
{
std::cout << "error!!!!!!!!!!" << std::endl;
......@@ -199,16 +177,11 @@ void CDynamixelSlave::start(void)
{
if(this->thread_server->get_thread_state(this->process_packets_thread_id)==attached)
this->thread_server->start_thread(this->process_packets_thread_id);
if(this->thread_server->get_thread_state(this->dynamixel_loop_thread_id)==attached)
this->thread_server->start_thread(this->dynamixel_loop_thread_id);
}
void CDynamixelSlave::stop(void)
{
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->dynamixel_loop_thread_id)==active || this->thread_server->get_thread_state(this->dynamixel_loop_thread_id)==starting)
this->thread_server->end_thread(this->dynamixel_loop_thread_id);
if(this->thread_server->get_thread_state(this->process_packets_thread_id)==active || this->thread_server->get_thread_state(this->process_packets_thread_id)==starting)
this->thread_server->end_thread(this->process_packets_thread_id);
}
......@@ -220,7 +193,7 @@ void CDynamixelSlave::dynamixel_loop_v1(void)
unsigned char *data,*write_data,read_data[64*1024];
unsigned int i=0;
data=this->packets[0];
data=this->new_packet;
id=this->get_target_id(data);
for(i=0;i<this->slaves.size();i++)
{
......@@ -325,7 +298,6 @@ void CDynamixelSlave::dynamixel_loop_v1(void)
}
}
}
this->packets.erase(this->packets.begin());
}
void CDynamixelSlave::dynamixel_loop_v2(void)
......@@ -335,7 +307,7 @@ void CDynamixelSlave::dynamixel_loop_v2(void)
unsigned char *data,*write_data,read_data[64*1024];
unsigned int i=0;
data=this->packets[0];
data=this->new_packet;
id=this->get_target_id(data);
for(i=0;i<this->slaves.size();i++)
{
......@@ -474,7 +446,6 @@ void CDynamixelSlave::dynamixel_loop_v2(void)
}
}
}
this->packets.erase(this->packets.begin());
}
unsigned char CDynamixelSlave::get_target_id(unsigned char *data)
......@@ -672,7 +643,7 @@ unsigned char CDynamixelSlave::bulk_read_id_present(unsigned char *data,unsigned
if(data[6+i*3+1]==id)
{
(*address)=data[6+i*3+2];
(*address)=data[6+i*3+3];
(*length)=data[6+i*3];
if(i==0)
return 0x00;
else
......@@ -691,7 +662,7 @@ unsigned char CDynamixelSlave::bulk_read_id_present(unsigned char *data,unsigned
if(data[8+i*5]==id)
{
(*address)=data[8+i*5+1]+data[9+i*5+1]*256;
(*length)=data[10+i*8+1]+data[11+i*5+1]*256;
(*length)=data[10+i*5+1]+data[11+i*5+1]*256;
if(i==0)
return 0x00;
else
......@@ -772,6 +743,10 @@ void CDynamixelSlave::send_status_packet(unsigned char id,dyn_error_t error,unsi
std::cout << "Error while reading the communication device" << std::endl;
this->comm_access.exit();
}
this->slave_answer_length=this->get_packet_length(status_data);
memcpy(this->slave_answer,status_data,this->slave_answer_length);
if(!this->event_server->event_is_set(this->slave_answer_event_id))
this->event_server->set_event(this->slave_answer_event_id);
}
void CDynamixelSlave::add_slave(unsigned char id, on_ping_fnct on_ping, on_rw_fnct on_read, on_rw_fnct on_write)
......@@ -849,7 +824,6 @@ 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)
......@@ -858,15 +832,8 @@ CDynamixelSlave::~CDynamixelSlave()
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="";
this->event_server->delete_event(this->finish_thread_event_id);
this->finish_thread_event_id="";
this->event_server->delete_event(this->new_packet_available_event_id);
this->new_packet_available_event_id="";
this->event_server->delete_event(this->slave_answer_event_id);
this->slave_answer_event_id="";
}
src/dynamixel_slave.h
+ 9
− 8
View file @ ab197094
......@@ -50,12 +50,12 @@ class CDynamixelSlave
* \brief
*
*/
std::string new_packet_available_event_id;
std::string finish_thread_event_id;
/**
* \brief
*
*/
std::string finish_thread_event_id;
std::string slave_answer_event_id;
/**
* \brief
*
......@@ -66,11 +66,6 @@ class CDynamixelSlave
*
*/
std::string process_packets_thread_id;
/**
* \brief
*
*/
std::string dynamixel_loop_thread_id;
/**
* \brief
*
......@@ -100,7 +95,13 @@ class CDynamixelSlave
* \brief
*
*/
std::vector<unsigned char *> packets;
unsigned char new_packet[1024];
/**
* \brief
*
*/
unsigned char slave_answer[1024];
unsigned int slave_answer_length;
protected:
/**
* \brief mutual exclusion mechanism to access the usb
......
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