diff --git a/src/dynamixel_slave.cpp b/src/dynamixel_slave.cpp
index c836c10087a8b451488a0da61584f41920fc6bd9..bdaad1c70b1cef354098a11ee1a8ec5717e409d9 100644
--- a/src/dynamixel_slave.cpp
+++ b/src/dynamixel_slave.cpp
@@ -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="";
}
diff --git a/src/dynamixel_slave.h b/src/dynamixel_slave.h
index 669b1be989f65c39f41e0744f0569a3f3d635f0b..f51d42b287445dde20d9ee21a5be23c9955a557f 100644
--- a/src/dynamixel_slave.h
+++ b/src/dynamixel_slave.h
@@ -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