From 7d91227eb0ce9de2ddac145b22fa363cf94894f6 Mon Sep 17 00:00:00 2001
From: Sergi Hernandez Juan <shernand@iri.upc.edu>
Date: Wed, 15 May 2019 14:37:47 +0200
Subject: [PATCH] Modified the Dynamixel server to allow multiple instances of
a single device.
---
src/dynamixelserver.cpp | 103 +++------------------------------
src/dynamixelserver.h | 11 +---
src/dynamixelserver_ftdi.cpp | 15 ++---
src/dynamixelserver_serial.cpp | 4 +-
4 files changed, 16 insertions(+), 117 deletions(-)
diff --git a/src/dynamixelserver.cpp b/src/dynamixelserver.cpp
index c48e6a8..37c91fc 100644
--- a/src/dynamixelserver.cpp
+++ b/src/dynamixelserver.cpp
@@ -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)
diff --git a/src/dynamixelserver.h b/src/dynamixelserver.h
index eba9a86..9c5559a 100644
--- a/src/dynamixelserver.h
+++ b/src/dynamixelserver.h
@@ -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
*
diff --git a/src/dynamixelserver_ftdi.cpp b/src/dynamixelserver_ftdi.cpp
index 19c2306..fad0e4e 100644
--- a/src/dynamixelserver_ftdi.cpp
+++ b/src/dynamixelserver_ftdi.cpp
@@ -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));
}
diff --git a/src/dynamixelserver_serial.cpp b/src/dynamixelserver_serial.cpp
index 94e44e0..26c6587 100644
--- a/src/dynamixelserver_serial.cpp
+++ b/src/dynamixelserver_serial.cpp
@@ -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));
}
--
GitLab