Commit 66c7041e authored by Sergi Hernandez's avatar Sergi Hernandez

Merge branch 'multi_instance'

parents 9c2a8f7d 0b9c30e8
......@@ -28,7 +28,8 @@ This package requires of the following system libraries and packages
* [cmake](https://www.cmake.org "CMake's Homepage"), a cross-platform build system.
* [doxygen](http://www.doxygen.org "Doxygen's Homepage") and [graphviz](http://www.graphviz.org "Graphviz's Homepage") to generate the documentation.
* stdc++ and pthread libraries.
* xsdcxx, W3C XML Schema to C++ Compiler
* [xsdcxx](https://www.codesynthesis.com/projects/xsd/) and libxerces-c-dev, XML parser and serializer.
* libboost-thread-dev, Boost thread development library.
Under linux all of these utilities are available in ready-to-use packages.
......
......@@ -6,38 +6,34 @@
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;
this->dyn_ver=dyn_ver;
this->comm_dev=NULL;
this->data_phase=false;
this->num_bytes=0;
this->length=0;
}
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;
static unsigned char packet[64*1024];
static int num_bytes=0,length=0;
// wait until the comm device becomes valid
while(!end)
{
......@@ -48,6 +44,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)
{
......@@ -56,102 +53,113 @@ 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();
std::cout << "Num data: " << num << std::endl;
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++)
{
std::cout << "num bytes: " << num_bytes << std::endl;
if(!data_phase)
try{
// process the incomming data
if(event_index==2)
{
slave->comm_access.enter();
num=slave->comm_dev->get_num_data();
read=slave->comm_dev->read(data,num);
if(read!=num)
{
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;
break;
default: break;
}
slave->comm_access.exit();
std::cout << "Error while reading the communication device" << std::endl;
}
else// data phase
else
{
std::cout << "data phase: " << length << std::endl;
packet[num_bytes]=data[i];
num_bytes++;
length--;
if(length==0)
slave->comm_access.exit();
for(i=0;i<num;i++)
{
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;
if(!slave->data_phase)
{
switch(slave->num_bytes)
{
case 0: if(data[i]==0xFF)
{
slave->packet[slave->num_bytes]=data[i];
slave->num_bytes++;
}
break;
case 1: if(data[i]==0xFF)
{
slave->packet[slave->num_bytes]=data[i];
slave->num_bytes++;
}
else
slave->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
{
slave->packet[slave->num_bytes]=data[i];
slave->num_bytes++;
}
else
slave->num_bytes=0;// ignore packet and restart synchronization
}
else if(data[i]!=0xFF)
{
slave->packet[slave->num_bytes]=data[i];
slave->num_bytes++;
}
break;
case 3: slave->packet[slave->num_bytes]=data[i];
if(slave->dyn_ver==dyn_version1)
{
slave->length=data[i];
slave->num_bytes++;
/* read packet_data */
slave->data_phase=true;
}
else
slave->num_bytes++;
break;
case 4: slave->packet[slave->num_bytes]=data[i];
slave->num_bytes++;
break;
case 5: slave->packet[slave->num_bytes]=data[i];
slave->num_bytes++;
slave->length=data[i];
break;
case 6: slave->packet[slave->num_bytes]=data[i];
slave->num_bytes++;
slave->length+=(data[i]<<8);
/* read packet_data */
slave->data_phase=true;
break;
default: break;
}
}
else// data phase
{
slave->packet[slave->num_bytes]=data[i];
slave->num_bytes++;
slave->length--;
if(slave->length==0)
{
slave->data_phase=false;
memcpy(slave->new_packet,slave->packet,slave->num_bytes);
if(slave->dyn_ver==dyn_version1)
slave->dynamixel_loop_v1();
else
slave->dynamixel_loop_v2();
slave->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){
}
}
}
......@@ -159,33 +167,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
{
// process the packets
if(slave->dyn_ver==dyn_version1)
slave->dynamixel_loop_v1();
else
slave->dynamixel_loop_v2();
}
}
pthread_exit(NULL);
}
void CDynamixelSlave::handle_error(unsigned char error)
{
std::cout << "error!!!!!!!!!!" << std::endl;
......@@ -195,16 +176,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);
}
......@@ -216,7 +192,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++)
{
......@@ -232,9 +208,7 @@ void CDynamixelSlave::dynamixel_loop_v1(void)
if(id!=0xFE)
this->send_status_packet(this->slaves[i].id,dyn_no_error,0,NULL);
break;
case dyn_read: std::cout << "length: " << this->get_read_length(data) << std::endl;
std::cout << "address: " << this->get_read_address(data) << std::endl;
error=this->slaves[i].on_read(this->get_read_address(data),this->get_read_length(data),read_data);
case dyn_read: error=this->slaves[i].on_read(this->get_read_address(data),this->get_read_length(data),read_data);
if(this->return_level!=no_return && id!=0xFE)
{
if(error==dyn_no_error)
......@@ -323,7 +297,6 @@ void CDynamixelSlave::dynamixel_loop_v1(void)
}
}
}
this->packets.erase(this->packets.begin());
}
void CDynamixelSlave::dynamixel_loop_v2(void)
......@@ -333,7 +306,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++)
{
......@@ -472,7 +445,6 @@ void CDynamixelSlave::dynamixel_loop_v2(void)
}
}
}
this->packets.erase(this->packets.begin());
}
unsigned char CDynamixelSlave::get_target_id(unsigned char *data)
......@@ -670,7 +642,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
......@@ -689,7 +661,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
......@@ -770,6 +742,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)
......@@ -847,16 +823,16 @@ void CDynamixelSlave::get_return_level(return_level_t level)
CDynamixelSlave::~CDynamixelSlave()
{
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="";
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,37 @@ class CDynamixelSlave
* \brief
*
*/
std::vector<unsigned char *> packets;
unsigned char new_packet[1024];
/**
* \brief
*
*/
unsigned char slave_answer[1024];
/**
* \brief
*
*/
unsigned int slave_answer_length;
/**
* \brief
*
*/
bool data_phase;
/**
* \brief
*
*/
unsigned char packet[64*1024];
/**
* \brief
*
*/
unsigned int num_bytes;
/**
* \brief
*
*/
unsigned int length;
protected:
/**
* \brief mutual exclusion mechanism to access the usb
......@@ -132,16 +157,6 @@ class CDynamixelSlave
*
*/
void send_status_packet(unsigned char id,unsigned char error, unsigned short int length, unsigned char *data);
/**
* \brief
*
*/
void start(void);
/**
* \brief
*
*/
void stop(void);
/**
* \brief
*
......@@ -297,6 +312,16 @@ class CDynamixelSlave
*
*/
void get_return_level(return_level_t level);
/**
* \brief
*
*/
void start(void);
/**
* \brief
*
*/
void stop(void);
/**
* \brief
*
......
......@@ -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){
}
}
......@@ -194,9 +194,7 @@ unsigned char CDynamixelServer::receive_status_packet_v1(unsigned char **data,un
do{
if((num=this->comm_dev->get_num_data())==0)
{
this->dynamixel_access.exit();
this->event_server->wait_all(events,20);
this->dynamixel_access.enter();
num=this->comm_dev->get_num_data();
}
if((read+num)>1024)
......@@ -217,9 +215,7 @@ unsigned char CDynamixelServer::receive_status_packet_v1(unsigned char **data,un
{
if((num=this->comm_dev->get_num_data())==0)
{
this->dynamixel_access.exit();
this->event_server->wait_all(events,20);
this->dynamixel_access.enter();
num=this->comm_dev->get_num_data();
}
if((read-start+num)>length)
......@@ -281,9 +277,7 @@ unsigned char CDynamixelServer::receive_status_packet_v2(unsigned char **data,un
do{
if((num=this->comm_dev->get_num_data())==0)
{
this->dynamixel_access.exit();
this->event_server->wait_all(events,20);
this->dynamixel_access.enter();
num=this->comm_dev->get_num_data();
}
if((read+num)>1024)
......@@ -304,9 +298,7 @@ unsigned char CDynamixelServer::receive_status_packet_v2(unsigned char **data,un
{
if((num=this->comm_dev->get_num_data())==0)
{
this->dynamixel_access.exit();
this->event_server->wait_all(events,20);
this->dynamixel_access.enter();
num=this->comm_dev->get_num_data();
}
if((read-start+num)>length)
......@@ -579,7 +571,6 @@ void CDynamixelServer::sync_packet_v2(unsigned char *data,unsigned int length,in
void *CDynamixelServer::scan_thread(void *param)
{
CDynamixelServer *dyn_server=(CDynamixelServer *)param;
TDynDevice device;
int freq=0,id=0;
bool end=false,found=false;
......@@ -618,15 +609,10 @@ void *CDynamixelServer::scan_thread(void *param)
}catch(CEventTimeoutException &e){
continue;
}
dyn_server->dynamixel_access.enter();
device.id=id;
device.version=dyn_server->scan_version;
device.used=false;
dyn_server->dynamixel_access.exit();
if(dyn_server->scan_version==dyn_version1)
dyn_server->devices_v1.push_back(device);
dyn_server->devices_v1.push_back(id);
else
dyn_server->devices_v2.push_back(device);
dyn_server->devices_v2.push_back(id);
found=true;
}
}
......@@ -735,15 +721,12 @@ int CDynamixelServer::get_num_devices(dyn_version_t version)
std::vector<int> CDynamixelServer::get_device_ids(dyn_version_t version)
{
std::vector<int> ids;
unsigned int i=0;
this->dynamixel_access.enter();
if(version==dyn_version1)
for(i=0;i<this->devices_v1.size();i++)
ids.push_back(this->devices_v1[i].id);
ids=this->devices_v1;
else
for(i=0;i<this->devices_v2.size();i++)
ids.push_back(this->devices_v2[i].id);
ids=this->devices_v2;
this->dynamixel_access.exit();
return ids;
......@@ -755,7 +738,6 @@ CDynamixel *CDynamixelServer::get_device(int dev_id,dyn_version_t version)
CDynamixel *dynamixel=NULL;
std::string name,serial;
bool updated=false;
TDynDevice device;
unsigned int i=0;
this->dynamixel_access.enter();
......@@ -765,35 +747,6 @@ CDynamixel *CDynamixelServer::get_device(int dev_id,dyn_version_t version)
/* handle exceptions */
throw CDynamixelServerException(_HERE_,"No communication device has been initialized.");