Commit ab197094 authored by Sergi Hernandez's avatar Sergi Hernandez

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
......@@ -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="";
}
......@@ -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
......
Markdown is supported
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