Commit 7d91227e authored by Sergi Hernandez's avatar Sergi Hernandez

Modified the Dynamixel server to allow multiple instances of a single device.

parent 9c2a8f7d
......@@ -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.");
}
else
{
if(version==dyn_version1)
{
for(i=0;i<this->devices_v1.size();i++)
if(this->devices_v1[i].id==dev_id)
{
if(this->devices_v1[i].used)
{
/* handle exceptions */
this->dynamixel_access.exit();
throw CDynamixelServerException(_HERE_,"Device has already been assigned.");
}
}
}
else
{
for(i=0;i<this->devices_v2.size();i++)
if(this->devices_v2[i].id==dev_id)
{
if(this->devices_v2[i].used)
{
/* handle exceptions */
this->dynamixel_access.exit();
throw CDynamixelServerException(_HERE_,"Device has already been assigned.");
}
}
}
}
this->dynamixel_access.exit();
if(dynamixel==NULL)
{
......@@ -819,37 +772,21 @@ CDynamixel *CDynamixelServer::get_device(int dev_id,dyn_version_t version)
{
for(i=0;i<devices_v1.size();i++)
{
if(this->devices_v1[i].id==dev_id)
{
this->devices_v1[i].used=true;
if(this->devices_v1[i]==dev_id)
updated=true;
}
}
if(!updated)
{
device.id=dev_id;
device.version=version;
device.used=true;
this->devices_v1.push_back(device);
}
this->devices_v1.push_back(dev_id);
}
else
{
for(i=0;i<devices_v2.size();i++)
{
if(this->devices_v2[i].id==dev_id)
{
this->devices_v2[i].used=true;
if(this->devices_v2[i]==dev_id)
updated=true;
}
}
if(!updated)
{
device.id=dev_id;
device.version=version;
device.used=true;
this->devices_v2.push_back(device);
}
this->devices_v2.push_back(dev_id);
}
}
this->dynamixel_access.exit();
......@@ -859,30 +796,6 @@ CDynamixel *CDynamixelServer::get_device(int dev_id,dyn_version_t version)
void CDynamixelServer::free_device(int dev_id,dyn_version_t version)
{
unsigned int i=0;
if(version==dyn_version1)
{
for(i=0;i<this->devices_v1.size();i++)
{
if(this->devices_v1[i].id==dev_id)
{
if(this->devices_v1[i].used)
this->devices_v1[i].used=false;
}
}
}
else
{
for(i=0;i<this->devices_v2.size();i++)
{
if(this->devices_v2[i].id==dev_id)
{
if(this->devices_v2[i].used)
this->devices_v2[i].used=false;
}
}
}
}
void CDynamixelServer::action(dyn_version_t version)
......
......@@ -37,13 +37,6 @@ class CDynamixel;
*/
typedef enum {dyn_created,dyn_scanning,dyn_scan_done} dynamixel_state;
typedef struct
{
unsigned char id;
dyn_version_t version;
bool used;
}TDynDevice;
/**
* \brief
*
......@@ -137,12 +130,12 @@ class CDynamixelServer
* \brief
*
*/
std::vector<TDynDevice> devices_v1;
std::vector<int> devices_v1;
/**
* \brief
*
*/
std::vector<TDynDevice> devices_v2;
std::vector<int> devices_v2;
/**
* \brief
*
......
......@@ -106,19 +106,12 @@ void CDynamixelServerFTDI::config(std::string &filename)
int CDynamixelServerFTDI::get_num_buses(void)
{
CFTDIServer *ftdi_server=CFTDIServer::instance();
int num_dev=0,i=0,num_buses=0;
std::string description;
int num_buses=0;
this->dynamixel_access.enter();
try{
ftdi_server->scan_bus();// rescan all the present FTDI devices
num_dev=ftdi_server->get_num_devices();
for(i=0;i<num_dev;i++)
{
description=ftdi_server->get_description(i);
if(description=="FT232R USB UART")
num_buses++;
}
num_buses=ftdi_server->get_num_devices();
}catch(CException &e){
/* handle exceptions */
this->dynamixel_access.exit();
......@@ -235,7 +228,7 @@ void CDynamixelServerFTDI::set_baudrate(int baudrate)
data.resize(this->devices_v1.size());
for(i=0;i<this->devices_v1.size();i++)
{
servo_ids.push_back(devices_v1[i].id);
servo_ids.push_back(devices_v1[i]);
data[i].clear();
data[i].push_back(((2000000/baudrate)-1));
}
......@@ -247,7 +240,7 @@ void CDynamixelServerFTDI::set_baudrate(int baudrate)
data.resize(this->devices_v2.size());
for(i=0;i<this->devices_v2.size();i++)
{
servo_ids.push_back(devices_v2[i].id);
servo_ids.push_back(devices_v2[i]);
data[i].clear();
data[i].push_back(((2000000/baudrate)-1));
}
......
......@@ -110,7 +110,7 @@ void CDynamixelServerSerial::set_baudrate(int baudrate)
data.resize(this->devices_v1.size());
for(i=0;i<this->devices_v1.size();i++)
{
servo_ids.push_back(devices_v1[i].id);
servo_ids.push_back(devices_v1[i]);
data[i].clear();
data[i].push_back(((2000000/baudrate)-1));
}
......@@ -122,7 +122,7 @@ void CDynamixelServerSerial::set_baudrate(int baudrate)
data.resize(this->devices_v2.size());
for(i=0;i<this->devices_v2.size();i++)
{
servo_ids.push_back(devices_v2[i].id);
servo_ids.push_back(devices_v2[i]);
data[i].clear();
data[i].push_back(((2000000/baudrate)-1));
}
......
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