diff --git a/src/CMakeLists.txt b/src/CMakeLists.txt
index 9bb4ad59c360538c2a837cfd5f0876433709891c..d64b28b1a68904daecf0be3dd83fbfebae743e7c 100644
--- a/src/CMakeLists.txt
+++ b/src/CMakeLists.txt
@@ -1,7 +1,9 @@
+ADD_SUBDIRECTORY(xml)
+
# edit the following line to add all the source code files of the library
-SET(sources dynamixel.cpp dynamixelserver.cpp dynamixelexceptions.cpp)
+SET(sources dynamixel.cpp dynamixelserver.cpp dynamixelserver_ftdi.cpp dynamixelserver_serial.cpp dynamixelexceptions.cpp)
# edit the following line to add all the header files of the library
-SET(headers dynamixel.h dynamixelserver.h dynamixelexceptions.h)
+SET(headers dynamixel.h dynamixelserver.h dynamixelserver_ftdi.h dynamixelserver_serial.h dynamixelexceptions.h)
INCLUDE_DIRECTORIES(.)
@@ -13,11 +15,16 @@ FIND_PACKAGE(comm REQUIRED)
INCLUDE_DIRECTORIES(${iriutils_INCLUDE_DIR})
INCLUDE_DIRECTORIES(${comm_INCLUDE_DIR})
-ADD_LIBRARY(dynamixel SHARED ${sources})
+SET_SOURCE_FILES_PROPERTIES(${XSD_SOURCES} PROPERTIES GENERATED 1)
+
+ADD_LIBRARY(dynamixel SHARED ${sources} ${XSD_SOURCES})
#edit the following line to add the necessary system libraries (if any)
TARGET_LINK_LIBRARIES(dynamixel ${iriutils_LIBRARY})
TARGET_LINK_LIBRARIES(dynamixel ${comm_LIBRARY})
+TARGET_LINK_LIBRARIES(dynamixel ${XSD_LIBRARY})
+
+ADD_DEPENDENCIES(dynamixel xsd_files_gen)
INSTALL(TARGETS dynamixel
RUNTIME DESTINATION bin
diff --git a/src/dynamixel.cpp b/src/dynamixel.cpp
index 35b32428449dc30d170d667ae39ac671778a01c2..1d55e9be5fb0b7d3e1ed60a5ce6870a0756bf2db 100644
--- a/src/dynamixel.cpp
+++ b/src/dynamixel.cpp
@@ -9,7 +9,8 @@
CDynamixel::CDynamixel(std::string& cont_id)
{
this->event_server=CEventServer::instance();
- this->usb_dev=NULL;
+ this->dyn_server=NULL;
+ this->comm_dev=NULL;
this->usb_access=NULL;
this->usb_rx_event_id="";
this->node_address=-1;
@@ -43,10 +44,10 @@ void CDynamixel::send_instruction_packet_v1(dyn_inst_t inst,unsigned char *data,
// byte stuffing
packet[length-1]=0x00;
packet[length-1]=CDynamixelServer::compute_checksum_v1(packet,length);
- if(this->usb_dev!=NULL)
+ if(this->comm_dev!=NULL)
{
this->usb_access->enter();
- if(this->usb_dev->write(packet,length)!=length)
+ if(this->comm_dev->write(packet,length)!=length)
{
/* handle exceptions */
this->usb_access->exit();
@@ -91,10 +92,10 @@ void CDynamixel::send_instruction_packet_v2(dyn_inst_t inst,unsigned char *data,
crc=CDynamixelServer::compute_checksum_v2(packet,length-2);
packet[length-2]=crc%256;
packet[length-1]=crc/256;
- if(this->usb_dev!=NULL)
+ if(this->comm_dev!=NULL)
{
this->usb_access->enter();
- if(this->usb_dev->write(packet,length)!=length)
+ if(this->comm_dev->write(packet,length)!=length)
{
/* handle exceptions */
this->usb_access->exit();
@@ -115,26 +116,26 @@ unsigned char CDynamixel::receive_status_packet_v1(unsigned char **data,unsigned
unsigned char data_int[1024];
int num=0,read=0,length,start=0;
- if(this->usb_dev!=NULL)
+ if(this->comm_dev!=NULL)
{
try{
this->usb_access->enter();
- events.push_back(this->usb_dev->get_rx_event_id());
+ events.push_back(this->comm_dev->get_rx_event_id());
// read up to the length field
do{
- if((num=this->usb_dev->get_num_data())==0)
+ if((num=this->comm_dev->get_num_data())==0)
{
- this->event_server->wait_all(events,50);
- num=this->usb_dev->get_num_data();
+ this->event_server->wait_all(events,500);
+ num=this->comm_dev->get_num_data();
}
if((read+num)>1024)
{
- this->usb_dev->read(&data_int[read],1024-read);
+ this->comm_dev->read(&data_int[read],1024-read);
read=1024;
}
else
{
- this->usb_dev->read(&data_int[read],num);
+ this->comm_dev->read(&data_int[read],num);
read+=num;
}
this->sync_packet_v1(data_int,read,&start);
@@ -143,19 +144,19 @@ unsigned char CDynamixel::receive_status_packet_v1(unsigned char **data,unsigned
// read the remaining of the packet
while((read-start)<length)
{
- if((num=this->usb_dev->get_num_data())==0)
+ if((num=this->comm_dev->get_num_data())==0)
{
- this->event_server->wait_all(events,50);
- num=this->usb_dev->get_num_data();
+ this->event_server->wait_all(events,500);
+ num=this->comm_dev->get_num_data();
}
if((read-start+num)>length)
{
- this->usb_dev->read(&data_int[read],length-read);
+ this->comm_dev->read(&data_int[read],length-read);
read=length;
}
else
{
- this->usb_dev->read(&data_int[read],num);
+ this->comm_dev->read(&data_int[read],num);
read+=num;
}
}
@@ -200,29 +201,29 @@ unsigned char CDynamixel::receive_status_packet_v2(unsigned char **data,unsigned
{
std::list<std::string> events;
unsigned char data_int[256];
- int num=0,read=0,length,start=0;
+ int num=0,read=0,length=0,start=0;
unsigned short int crc;
- if(this->usb_dev!=NULL)
+ if(this->comm_dev!=NULL)
{
try{
this->usb_access->enter();
- events.push_back(this->usb_dev->get_rx_event_id());
+ events.push_back(this->comm_dev->get_rx_event_id());
// read up to the length field
do{
- if((num=this->usb_dev->get_num_data())==0)
+ if((num=this->comm_dev->get_num_data())==0)
{
- this->event_server->wait_all(events,50);
- num=this->usb_dev->get_num_data();
+ this->event_server->wait_all(events,500);
+ num=this->comm_dev->get_num_data();
}
if((read+num)>256)
{
- this->usb_dev->read(&data_int[read],256-read);
+ this->comm_dev->read(&data_int[read],256-read);
read=256;
}
else
{
- this->usb_dev->read(&data_int[read],num);
+ this->comm_dev->read(&data_int[read],num);
read+=num;
}
this->sync_packet_v2(data_int,read,&start);
@@ -231,19 +232,19 @@ unsigned char CDynamixel::receive_status_packet_v2(unsigned char **data,unsigned
// read the remaining of the packet
while((read-start)<length)
{
- if((num=this->usb_dev->get_num_data())==0)
+ if((num=this->comm_dev->get_num_data())==0)
{
- this->event_server->wait_all(events,50);
- num=this->usb_dev->get_num_data();
+ this->event_server->wait_all(events,500);
+ num=this->comm_dev->get_num_data();
}
if((read-start+num)>256)
{
- this->usb_dev->read(&data_int[read],256-read);
+ this->comm_dev->read(&data_int[read],256-read);
read=256;
}
else
{
- this->usb_dev->read(&data_int[read],num);
+ this->comm_dev->read(&data_int[read],num);
read+=num;
}
}
@@ -256,7 +257,7 @@ unsigned char CDynamixel::receive_status_packet_v2(unsigned char **data,unsigned
throw CDynamixelException(_HERE_,"Invalid Checksum",this->node_address);
}
// return the error
- if(length>11)
+ if(length>10)
{
*data=new unsigned char[length-11];
memcpy(*data,&data_int[start+9],length-11);
@@ -312,7 +313,7 @@ void CDynamixel::handle_error(unsigned char error)
void CDynamixel::set_baudrate(int baudrate)
{
- if(this->usb_dev!=NULL)
+ if(this->comm_dev!=NULL)
{
baudrate=((2000000/baudrate)-1);
this->write_byte_register(this->baudrate_register,baudrate);
@@ -326,12 +327,10 @@ void CDynamixel::set_baudrate(int baudrate)
void CDynamixel::set_id(unsigned char id)
{
- CDynamixelServer *dyn_server=CDynamixelServer::instance();
-
- if(this->usb_dev!=NULL)
+ if(this->comm_dev!=NULL)
{
try{
- dyn_server->ping(id);
+ this->dyn_server->ping(id);
throw CDynamixelException(_HERE_,"There already exists a dynamixel device with the desired id.",id);
}catch(CEventTimeoutException &e){
this->write_byte_register(this->id_register,id);
@@ -447,11 +446,11 @@ void CDynamixel::resync(void)
// write to the dynamixel bus until it returns a packet
do{
try{
- this->usb_dev->write(&foo,1);
+ this->comm_dev->write(&foo,1);
this->event_server->wait_all(events,100);
- num=this->usb_dev->get_num_data();
+ num=this->comm_dev->get_num_data();
data=new unsigned char[num];
- this->usb_dev->read(data,num);
+ this->comm_dev->read(data,num);
delete[] data;
synced=true;
}catch(CEventTimeoutException &e){
@@ -803,7 +802,7 @@ void CDynamixel::set_baudrate_register(unsigned char reg)
CDynamixel::~CDynamixel()
{
- this->usb_dev=NULL;
+ this->comm_dev=NULL;
this->usb_access=NULL;
this->usb_rx_event_id="";
this->node_address=-1;
diff --git a/src/dynamixel.h b/src/dynamixel.h
index 6bc72295e4010c19d241b24cb68dd50797151eaa..a361d6a6fc285df5f514b393cb886e18c7745fd3 100644
--- a/src/dynamixel.h
+++ b/src/dynamixel.h
@@ -3,16 +3,18 @@
//#include "dynamixelserver.h"
#include "eventserver.h"
-#include "ftdimodule.h"
+#include "comm.h"
#include "mutex.h"
#include <stdio.h>
#include <stdlib.h>
#include <string>
#include <vector>
+class CDynamixelServer;
+
const int NUM_RETRIES=10;
-typedef enum {dyn_version1,dyn_version2} dyn_version_t;
+typedef enum {dyn_version1=1,dyn_version2=2} dyn_version_t;
typedef enum {dyn_ping=0x01,
dyn_read=0x02,
@@ -37,11 +39,16 @@ class CDynamixel
{
private:
friend class CDynamixelServer;
+ /**
+ * \brief
+ *
+ */
+ CDynamixelServer *dyn_server;
/**
* \brief Handle to the communications device
*
*/
- CFTDI *usb_dev;
+ CComm *comm_dev;
/**
* \brief
*
diff --git a/src/dynamixelserver.cpp b/src/dynamixelserver.cpp
index 9accb24669575a02cb80b13f56fabecf96832282..d9c6cd2b599996aa4d4e17a2cefce08a1513fc1c 100644
--- a/src/dynamixelserver.cpp
+++ b/src/dynamixelserver.cpp
@@ -1,7 +1,6 @@
#include "dynamixelexceptions.h"
#include "dynamixelserver.h"
#include "eventexceptions.h"
-#include "ftdiserver.h"
#include <sstream>
const unsigned short crc_table[256] = {
@@ -39,8 +38,6 @@ const unsigned short crc_table[256] = {
0x8213, 0x0216, 0x021C, 0x8219, 0x0208, 0x820D, 0x8207, 0x0202
};
-CDynamixelServer *CDynamixelServer::pinstance=NULL;
-
CDynamixelServer::CDynamixelServer()
{
this->event_server=CEventServer::instance();
@@ -58,32 +55,10 @@ CDynamixelServer::CDynamixelServer()
this->devices_v1.clear();
this->devices_v2.clear();
this->comm_dev=NULL;
- this->bus_info.baud_rate=-1;
- this->bus_info.bus_id=-1;
- this->bus_info.serial="";
this->scan_version=dyn_version1;
this->scan_error="No error";
}
-CDynamixelServer::CDynamixelServer(const CDynamixelServer& object)
-{
-
-}
-
-CDynamixelServer& CDynamixelServer::operator = (const CDynamixelServer& object)
-{
- return *this->pinstance;
-}
-
-CDynamixelServer *CDynamixelServer::instance(void)
-{
- if (CDynamixelServer::pinstance == NULL)
- {
- CDynamixelServer::pinstance = new CDynamixelServer(); // Creamos la instancia
- }
- return CDynamixelServer::pinstance; // Retornamos la dirección de la instancia
-}
-
void CDynamixelServer::send_instruction_packet_v1(dyn_inst_t inst,unsigned char *data,unsigned char len,unsigned char id)
{
unsigned char *packet=NULL;
@@ -607,8 +582,6 @@ void CDynamixelServer::sync_packet_v2(unsigned char *data,unsigned int length,in
void *CDynamixelServer::scan_thread(void *param)
{
CDynamixelServer *dyn_server=(CDynamixelServer *)param;
- CFTDIServer *ftdi_server=CFTDIServer::instance();
- std::string serial;
TDynDevice device;
int freq=0,id=0;
bool end=false,found=false;
@@ -617,11 +590,16 @@ void *CDynamixelServer::scan_thread(void *param)
while(!end)
{
try{
- if(dyn_server->bus_info.bus_id!=-1)
- serial=ftdi_server->get_serial_number(dyn_server->bus_info.bus_id);
- else
- serial=dyn_server->bus_info.serial;
- dyn_server->comm_dev=ftdi_server->get_device(serial);
+ if(dyn_server->comm_dev==NULL)
+ {
+ dyn_server->dynamixel_access.enter();
+ dyn_server->state=dyn_created;
+ dyn_server->scan_error="communication device not initialized";
+ dyn_server->event_server->set_event(dyn_server->scan_error_event_id);
+ dyn_server->close();
+ dyn_server->dynamixel_access.exit();
+ pthread_exit(NULL);
+ }
for(freq=0;freq<9;freq++)
{
dyn_server->set_baudrate(frequencies[freq]);
@@ -645,7 +623,6 @@ void *CDynamixelServer::scan_thread(void *param)
continue;
}
dyn_server->dynamixel_access.enter();
- dyn_server->bus_info.baud_rate=frequencies[freq];
device.id=id;
device.version=dyn_server->scan_version;
device.used=false;
@@ -685,150 +662,12 @@ void *CDynamixelServer::scan_thread(void *param)
pthread_exit(NULL);
}
-void CDynamixelServer::config_bus(int bus_id, int baudrate)
-{
- TFTDIconfig ftdi_config;
-
- // set the desired bus identifier
- this->set_bus_id(bus_id);
- // set the desired baudrate for the bus
- /* reconfigure the communciations device */
- ftdi_config.word_length=8;
- ftdi_config.stop_bits=1;
- ftdi_config.parity=0;
- ftdi_config.read_timeout = 1000;
- ftdi_config.write_timeout = 1000;
- ftdi_config.latency_timer = 1;
- ftdi_config.baud_rate=baudrate;
- this->comm_dev->config(&ftdi_config);
- this->bus_info.baud_rate=baudrate;
-// this->set_baudrate(baudrate);
-}
-
-void CDynamixelServer::config_bus(std::string &bus_id, int baudrate)
-{
- TFTDIconfig ftdi_config;
-
- // set the desired bus identifier
- this->set_bus_id(bus_id);
- // set the desired baudrate for the bus
- /* reconfigure the communciations device */
- ftdi_config.word_length=8;
- ftdi_config.stop_bits=1;
- ftdi_config.parity=0;
- ftdi_config.read_timeout = 1000;
- ftdi_config.write_timeout = 1000;
- ftdi_config.latency_timer = 1;
- ftdi_config.baud_rate=baudrate;
- this->comm_dev->config(&ftdi_config);
- this->bus_info.baud_rate=baudrate;
-// this->set_baudrate(baudrate);
-}
-
-int CDynamixelServer::get_num_buses(void)
-{
- CFTDIServer *ftdi_server=CFTDIServer::instance();
- int num_dev=0,i=0,num_buses=0;
- std::string description;
-
- 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++;
- }
- }catch(CException &e){
- /* handle exceptions */
- this->dynamixel_access.exit();
- throw;
- }
- this->dynamixel_access.exit();
-
- return num_buses;
-}
-
-int CDynamixelServer::get_bus_id(void)
-{
- return this->bus_info.bus_id;
-}
-
-std::string &CDynamixelServer::get_bus_serial(void)
-{
- return this->bus_info.serial;
-}
-
-void CDynamixelServer::set_bus_id(int bus_id)
-{
- CFTDIServer *ftdi_server=CFTDIServer::instance();
- std::string serial;
-
- if(bus_id>(this->get_num_buses()-1))
- {
- /* handle exception */
- throw CDynamixelServerException(_HERE_,"Invalid bus identifier");
- }
- else
- {
- if(this->bus_info.bus_id!=bus_id)
- {
- if(this->comm_dev!=NULL)
- {
- this->comm_dev->close();
- delete this->comm_dev;
- this->comm_dev=NULL;
- }
- serial=ftdi_server->get_serial_number(bus_id);
- this->comm_dev=ftdi_server->get_device(serial);
- this->bus_info.bus_id=bus_id;
- this->bus_info.serial=serial;
- }
- }
-}
-
-void CDynamixelServer::set_bus_id(std::string &bus_id)
-{
- CFTDIServer *ftdi_server=CFTDIServer::instance();
-
- if(bus_id.size()==0)
- {
- /* handle exception */
- throw CDynamixelServerException(_HERE_,"Invalid bus serial number");
- }
- else
- {
- if(this->bus_info.serial!=bus_id)
- {
- if(this->comm_dev!=NULL)
- {
- this->comm_dev->close();
- delete this->comm_dev;
- this->comm_dev=NULL;
- }
- this->comm_dev=ftdi_server->get_device(bus_id);
- if(this->comm_dev==NULL)
- {
- /* handle exception */
- throw CDynamixelServerException(_HERE_,"No device found with the given serial number");
- }
- else
- {
- this->bus_info.bus_id=-1;
- this->bus_info.serial=bus_id;
- }
- }
- }
-}
-
void CDynamixelServer::start_scan(dyn_version_t version)
{
- if(this->bus_info.bus_id==-1 && this->bus_info.serial.size()==0)
+ if(this->comm_dev==NULL)
{
/* handle exceptions */
- throw CDynamixelServerException(_HERE_,"No bus has been selected.");
+ throw CDynamixelServerException(_HERE_,"No communication device has been initialized.");
}
else
{
@@ -849,14 +688,12 @@ void CDynamixelServer::stop_scan(void)
{
this->event_server->set_event(this->stop_scan_event_id);
this->thread_server->end_thread(this->scan_thread_id);
- this->bus_info.bus_id=-1;
- this->bus_info.serial="";
}
}
bool CDynamixelServer::is_scan_done(void)
{
- if(this->bus_info.bus_id!=-1 || this->bus_info.serial.size()>0)
+ if(this->comm_dev!=NULL)
{
if(this->state==dyn_scan_done)
return true;
@@ -866,7 +703,7 @@ bool CDynamixelServer::is_scan_done(void)
else
{
/* handle exceptions */
- throw CDynamixelServerException(_HERE_,"No bus has been selected.");
+ throw CDynamixelServerException(_HERE_,"No communication device has been initialized.");
}
}
@@ -885,17 +722,6 @@ std::string CDynamixelServer::get_scan_error(void)
return this->scan_error;
}
-int CDynamixelServer::get_baudrate(void)
-{
- int baud_rate=0;
-
- this->dynamixel_access.enter();
- baud_rate=this->bus_info.baud_rate;
- this->dynamixel_access.exit();
-
- return baud_rate;
-}
-
int CDynamixelServer::get_num_devices(dyn_version_t version)
{
int num=0;
@@ -937,11 +763,11 @@ CDynamixel *CDynamixelServer::get_device(int dev_id,dyn_version_t version)
unsigned int i=0;
this->dynamixel_access.enter();
- if(this->bus_info.bus_id==-1 && this->bus_info.serial.size()==0)
+ if(this->comm_dev==NULL)
{
this->dynamixel_access.exit();
/* handle exceptions */
- throw CDynamixelServerException(_HERE_,"No bus has been selected.");
+ throw CDynamixelServerException(_HERE_,"No communication device has been initialized.");
}
else
{
@@ -976,68 +802,61 @@ CDynamixel *CDynamixelServer::get_device(int dev_id,dyn_version_t version)
if(dynamixel==NULL)
{
// try to ping the desired device
- if(this->bus_info.baud_rate==-1)
- {
+ try{
+ this->ping(dev_id,500,version);
+ }catch(...){
/* handle exceptions */
- throw CDynamixelServerException(_HERE_,"No baudrate has been selected.");
+ throw CDynamixelServerException(_HERE_,"No Dynamixel device found with the specified identifier");
}
- else
+ this->dynamixel_access.enter();
+ device_name.str("");
+ device_name << "dynamixel_bus_" << this->comm_dev->get_id() << "_dev_" << dev_id << "_v" << version;
+ name=device_name.str();
+ dynamixel=new CDynamixel(name);
+ dynamixel->dyn_server=this;
+ dynamixel->comm_dev=this->comm_dev;
+ dynamixel->usb_access=&this->dynamixel_access;
+ dynamixel->node_address=dev_id;
+ dynamixel->version=version;
+ dynamixel->usb_rx_event_id=this->comm_dev->get_rx_event_id();
+ if(version==dyn_version1)
{
- try{
- this->ping(dev_id,500,version);
- }catch(...){
- /* handle exceptions */
- throw CDynamixelServerException(_HERE_,"No Dynamixel device found with the specified identifier");
- }
- this->dynamixel_access.enter();
- device_name.str("");
- device_name << "dynamixel_bus_" << this->bus_info.bus_id << "_dev_" << dev_id << "_v" << version;
- name=device_name.str();
- dynamixel=new CDynamixel(name);
- dynamixel->usb_dev=this->comm_dev;
- dynamixel->usb_access=&this->dynamixel_access;
- dynamixel->node_address=dev_id;
- dynamixel->version=version;
- dynamixel->usb_rx_event_id=this->comm_dev->get_rx_event_id();
- if(version==dyn_version1)
+ for(i=0;i<devices_v1.size();i++)
{
- for(i=0;i<devices_v1.size();i++)
- {
- if(this->devices_v1[i].id==dev_id)
- {
- this->devices_v1[i].used=true;
- updated=true;
- }
- }
- if(!updated)
+ if(this->devices_v1[i].id==dev_id)
{
- device.id=dev_id;
- device.version=version;
- device.used=true;
- this->devices_v1.push_back(device);
+ this->devices_v1[i].used=true;
+ updated=true;
}
}
- else
+ if(!updated)
{
- for(i=0;i<devices_v2.size();i++)
- {
- if(this->devices_v2[i].id==dev_id)
- {
- this->devices_v2[i].used=true;
- updated=true;
- }
- }
- if(!updated)
+ device.id=dev_id;
+ device.version=version;
+ device.used=true;
+ this->devices_v1.push_back(device);
+ }
+ }
+ else
+ {
+ for(i=0;i<devices_v2.size();i++)
+ {
+ if(this->devices_v2[i].id==dev_id)
{
- device.id=dev_id;
- device.version=version;
- device.used=true;
- this->devices_v2.push_back(device);
+ this->devices_v2[i].used=true;
+ updated=true;
}
}
- this->dynamixel_access.exit();
+ if(!updated)
+ {
+ device.id=dev_id;
+ device.version=version;
+ device.used=true;
+ this->devices_v2.push_back(device);
+ }
}
}
+ this->dynamixel_access.exit();
return dynamixel;
}
@@ -1070,71 +889,6 @@ void CDynamixelServer::free_device(int dev_id,dyn_version_t version)
}
}
-void CDynamixelServer::set_baudrate(int baudrate)
-{
- std::vector< std::vector<unsigned char> > data;
- std::vector<unsigned char> servo_ids;
- TFTDIconfig ftdi_config;
- unsigned int i=0;
-
- if(this->comm_dev!=NULL)
- {
- if(baudrate <= 0 || baudrate > 1000000)
- {
- /* handle exceptions */
- throw CDynamixelServerException(_HERE_,"Invalid baudrate");
- }
- else
- {
- try{
- if(this->devices_v1.size()>0)
- {
- servo_ids.clear();
- data.resize(this->devices_v1.size());
- for(i=0;i<this->devices_v1.size();i++)
- {
- servo_ids.push_back(devices_v1[i].id);
- data[i].clear();
- data[i].push_back(((2000000/baudrate)-1));
- }
- this->write_sync(servo_ids,0x04,data,dyn_version1);
- }
- if(this->devices_v2.size()>0)
- {
- servo_ids.clear();
- data.resize(this->devices_v2.size());
- for(i=0;i<this->devices_v2.size();i++)
- {
- servo_ids.push_back(devices_v2[i].id);
- data[i].clear();
- data[i].push_back(((2000000/baudrate)-1));
- }
- this->write_sync(servo_ids,0x04,data,dyn_version2);
- }
- sleep(1);
- /* reconfigure the communciations device */
- ftdi_config.word_length=8;
- ftdi_config.stop_bits=1;
- ftdi_config.parity=0;
- ftdi_config.read_timeout = 1000;
- ftdi_config.write_timeout = 1000;
- ftdi_config.latency_timer = 1;
- ftdi_config.baud_rate=baudrate;
- this->comm_dev->config(&ftdi_config);
- this->bus_info.baud_rate=baudrate;
- }catch(CException &e){
- /* handle exceptions */
- throw;
- }
- }
- }
- else
- {
- /* handle exceptions */
- throw CDynamixelServerException(_HERE_,"The communication device is not ready to send information");
- }
-}
-
void CDynamixelServer::action(dyn_version_t version)
{
if(version==dyn_version1)
@@ -1205,6 +959,7 @@ void CDynamixelServer::ping(int dev_id,int time,dyn_version_t version,unsigned s
*fw_ver=data[2];
if(data!=NULL)
delete[] data;
+ done=true;
}catch(CEventTimeoutException &e){
this->dynamixel_access.enter();
for(i=0;i<32;i++)
@@ -1246,7 +1001,6 @@ void CDynamixelServer::close(void)
this->comm_dev=NULL;
}
this->state=dyn_created;
- this->bus_info.baud_rate=-1;
this->dynamixel_access.exit();
}
diff --git a/src/dynamixelserver.h b/src/dynamixelserver.h
index 763e3fc56f50ea5636e7adb4de1fbc31eedfbba5..eba9a8629fafb55df133023afa14f07fc4747251 100644
--- a/src/dynamixelserver.h
+++ b/src/dynamixelserver.h
@@ -4,7 +4,7 @@
#include <vector>
#include "mutex.h"
#include "dynamixel.h"
-#include "ftdimodule.h"
+#include "comm.h"
#include "eventserver.h"
#include "threadserver.h"
@@ -37,19 +37,6 @@ class CDynamixel;
*/
typedef enum {dyn_created,dyn_scanning,dyn_scan_done} dynamixel_state;
-/**
- * \brief Basic Dynamixel bus information
- *
- * This structure holds the identifier of the current bus used (in case there
- * exist multiple buses) and also the baudrate of the bus.
- */
-typedef struct
-{
- int baud_rate;
- int bus_id;
- std::string serial;
-}TBus_info;
-
typedef struct
{
unsigned char id;
@@ -70,11 +57,6 @@ const int frequencies[9]={1000000,500000,400000,250000,200000,115200,57600,19200
class CDynamixelServer
{
private:
- /**
- * \brief
- *
- */
- static CDynamixelServer *pinstance;
/**
* \brief
*
@@ -110,31 +92,6 @@ class CDynamixelServer
*
*/
dynamixel_state state;
- /**
- * \brief
- *
- */
- CFTDI *comm_dev;
- /**
- * \brief
- *
- */
- CMutex dynamixel_access;
- /**
- * \brief
- *
- */
- TBus_info bus_info;
- /**
- * \brief
- *
- */
- std::vector<TDynDevice> devices_v1;
- /**
- * \brief
- *
- */
- std::vector<TDynDevice> devices_v2;
/**
* \brief
*
@@ -180,42 +137,42 @@ class CDynamixelServer
* \brief
*
*/
- CDynamixelServer();
+ std::vector<TDynDevice> devices_v1;
/**
* \brief
*
*/
- CDynamixelServer(const CDynamixelServer &object);
+ std::vector<TDynDevice> devices_v2;
/**
* \brief
*
*/
- CDynamixelServer& operator = (const CDynamixelServer &object);
+ CMutex dynamixel_access;
/**
* \brief
*
*/
- static void *scan_thread(void *param);
+ CComm *comm_dev;
/**
* \brief
*
*/
- static void byte_stuffing(unsigned char *packet_in,int len_in,unsigned char *packet_out,int *len_out);
+ CDynamixelServer();
/**
* \brief
*
*/
- static void byte_destuffing(unsigned char *packet_in,int len_in,unsigned char *packet_out,int *len_out);
+ static void *scan_thread(void *param);
/**
* \brief
*
*/
- void set_bus_id(int bus_id);
+ static void byte_stuffing(unsigned char *packet_in,int len_in,unsigned char *packet_out,int *len_out);
/**
* \brief
*
*/
- void set_bus_id(std::string &bus_id);
+ static void byte_destuffing(unsigned char *packet_in,int len_in,unsigned char *packet_out,int *len_out);
/**
* \brief
*
@@ -226,47 +183,29 @@ class CDynamixelServer
* \brief
*
*/
- static CDynamixelServer *instance(void);
- /**
- * \brief
- *
- */
- static unsigned char compute_checksum_v1(unsigned char *packet,int len);
- /**
- * \brief
- *
- */
- static unsigned short int compute_checksum_v2(unsigned char *packet,int len);
- /**
- * \brief
- *
- */
- void config_bus(int bus_id,int baudrate);
+ virtual void set_baudrate(int baudrate)=0;
/**
* \brief
*
- */
- void config_bus(std::string &bus_id,int baudrate);
- /**
- * \brief
- *
- */
- void set_baudrate(int baudrate);
+ */
+ virtual int get_baudrate(void)=0;
+#ifdef _HAVE_XSD
/**
* \brief
*
- */
- int get_num_buses(void);
+ */
+ virtual void config(std::string &filename)=0;
+#endif
/**
* \brief
*
*/
- int get_bus_id(void);
+ static unsigned char compute_checksum_v1(unsigned char *packet,int len);
/**
* \brief
*
*/
- std::string &get_bus_serial(void);
+ static unsigned short int compute_checksum_v2(unsigned char *packet,int len);
/**
* \brief
*
@@ -297,11 +236,6 @@ class CDynamixelServer
*
*/
std::string get_scan_error(void);
- /**
- * \brief
- *
- */
- int get_baudrate(void);
/**
* \brief
*
diff --git a/src/dynamixelserver_ftdi.cpp b/src/dynamixelserver_ftdi.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..19c2306cf96c68431ebfdf37241a73edf84b5d4f
--- /dev/null
+++ b/src/dynamixelserver_ftdi.cpp
@@ -0,0 +1,278 @@
+#include "dynamixelexceptions.h"
+#include "dynamixelserver_ftdi.h"
+#include "eventexceptions.h"
+#include "ftdiserver.h"
+#include <sstream>
+#include <sys/types.h>
+#include <sys/stat.h>
+#include <unistd.h>
+#ifdef _HAVE_XSD
+#include "xml/dyn_server_ftdi_cfg_file.hxx"
+#endif
+
+CDynamixelServerFTDI *CDynamixelServerFTDI::pinstance=NULL;
+
+CDynamixelServerFTDI::CDynamixelServerFTDI()
+{
+ this->bus_info.baud_rate=-1;
+ this->bus_info.bus_id=-1;
+ this->bus_info.serial="";
+}
+
+CDynamixelServerFTDI::CDynamixelServerFTDI(const CDynamixelServerFTDI& object)
+{
+
+}
+
+CDynamixelServerFTDI& CDynamixelServerFTDI::operator = (const CDynamixelServerFTDI& object)
+{
+ return *this->pinstance;
+}
+
+CDynamixelServerFTDI *CDynamixelServerFTDI::instance(void)
+{
+ if (CDynamixelServerFTDI::pinstance == NULL)
+ {
+ CDynamixelServerFTDI::pinstance = new CDynamixelServerFTDI(); // Creamos la instancia
+ }
+ return CDynamixelServerFTDI::pinstance; // Retornamos la dirección de la instancia
+}
+
+void CDynamixelServerFTDI::config_bus(int bus_id, int baudrate)
+{
+ TFTDIconfig ftdi_config;
+
+ // set the desired bus identifier
+ this->set_bus_id(bus_id);
+ // set the desired baudrate for the bus
+ /* reconfigure the communciations device */
+ ftdi_config.word_length=8;
+ ftdi_config.stop_bits=1;
+ ftdi_config.parity=0;
+ ftdi_config.read_timeout = 1000;
+ ftdi_config.write_timeout = 1000;
+ ftdi_config.latency_timer = 1;
+ ftdi_config.baud_rate=baudrate;
+ this->comm_dev->config(&ftdi_config);
+ this->bus_info.baud_rate=baudrate;
+// this->set_baudrate(baudrate);
+}
+
+void CDynamixelServerFTDI::config_bus(std::string &bus_id, int baudrate)
+{
+ TFTDIconfig ftdi_config;
+
+ // set the desired bus identifier
+ this->set_bus_id(bus_id);
+ // set the desired baudrate for the bus
+ /* reconfigure the communciations device */
+ ftdi_config.word_length=8;
+ ftdi_config.stop_bits=1;
+ ftdi_config.parity=0;
+ ftdi_config.read_timeout = 1000;
+ ftdi_config.write_timeout = 1000;
+ ftdi_config.latency_timer = 1;
+ ftdi_config.baud_rate=baudrate;
+ this->comm_dev->config(&ftdi_config);
+ this->bus_info.baud_rate=baudrate;
+// this->set_baudrate(baudrate);
+}
+
+#ifdef _HAVE_XSD
+void CDynamixelServerFTDI::config(std::string &filename)
+{
+ struct stat buffer;
+
+ if(stat(filename.c_str(),&buffer)==0)
+ {
+ // try to open the specified file
+ try{
+ std::auto_ptr<dyn_server_ftdi_config_t> cfg(dyn_server_ftdi_config(filename.c_str(), xml_schema::flags::dont_validate));
+ this->config_bus(cfg->serial_num(),cfg->baudrate());
+ }catch (const xml_schema::exception& e){
+ std::ostringstream os;
+ os << e;
+ /* handle exceptions */
+ throw CDynamixelServerException(_HERE_,os.str());
+ }catch(CException &e){
+ throw e;
+ }
+ }
+ else
+ throw CDynamixelServerException(_HERE_,"The configuration file does not exist");
+}
+#endif
+
+int CDynamixelServerFTDI::get_num_buses(void)
+{
+ CFTDIServer *ftdi_server=CFTDIServer::instance();
+ int num_dev=0,i=0,num_buses=0;
+ std::string description;
+
+ 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++;
+ }
+ }catch(CException &e){
+ /* handle exceptions */
+ this->dynamixel_access.exit();
+ throw;
+ }
+ this->dynamixel_access.exit();
+
+ return num_buses;
+}
+
+int CDynamixelServerFTDI::get_bus_id(void)
+{
+ return this->bus_info.bus_id;
+}
+
+std::string &CDynamixelServerFTDI::get_bus_serial(void)
+{
+ return this->bus_info.serial;
+}
+
+void CDynamixelServerFTDI::set_bus_id(int bus_id)
+{
+ CFTDIServer *ftdi_server=CFTDIServer::instance();
+ std::string serial;
+
+ if(bus_id>(this->get_num_buses()-1))
+ {
+ /* handle exception */
+ throw CDynamixelServerException(_HERE_,"Invalid bus identifier");
+ }
+ else
+ {
+ if(this->bus_info.bus_id!=bus_id)
+ {
+ if(this->comm_dev!=NULL)
+ {
+ this->comm_dev->close();
+ delete this->comm_dev;
+ this->comm_dev=NULL;
+ }
+ serial=ftdi_server->get_serial_number(bus_id);
+ this->comm_dev=ftdi_server->get_device(serial);
+ this->bus_info.bus_id=bus_id;
+ this->bus_info.serial=serial;
+ }
+ }
+}
+
+void CDynamixelServerFTDI::set_bus_id(std::string &bus_id)
+{
+ CFTDIServer *ftdi_server=CFTDIServer::instance();
+
+ if(bus_id.size()==0)
+ {
+ /* handle exception */
+ throw CDynamixelServerException(_HERE_,"Invalid bus serial number");
+ }
+ else
+ {
+ if(this->bus_info.serial!=bus_id)
+ {
+ if(this->comm_dev!=NULL)
+ {
+ this->comm_dev->close();
+ delete this->comm_dev;
+ this->comm_dev=NULL;
+ }
+ this->comm_dev=ftdi_server->get_device(bus_id);
+ if(this->comm_dev==NULL)
+ {
+ /* handle exception */
+ throw CDynamixelServerException(_HERE_,"No device found with the given serial number");
+ }
+ else
+ {
+ this->bus_info.bus_id=-1;
+ this->bus_info.serial=bus_id;
+ }
+ }
+ }
+}
+
+int CDynamixelServerFTDI::get_baudrate(void)
+{
+ int baud_rate=0;
+
+ this->dynamixel_access.enter();
+ baud_rate=this->bus_info.baud_rate;
+ this->dynamixel_access.exit();
+
+ return baud_rate;
+}
+
+void CDynamixelServerFTDI::set_baudrate(int baudrate)
+{
+ std::vector< std::vector<unsigned char> > data;
+ std::vector<unsigned char> servo_ids;
+ TFTDIconfig ftdi_config;
+ unsigned int i=0;
+
+ if(this->comm_dev!=NULL)
+ {
+ if(baudrate <= 0 || baudrate > 1000000)
+ {
+ /* handle exceptions */
+ throw CDynamixelServerException(_HERE_,"Invalid baudrate");
+ }
+ else
+ {
+ try{
+ if(this->devices_v1.size()>0)
+ {
+ servo_ids.clear();
+ data.resize(this->devices_v1.size());
+ for(i=0;i<this->devices_v1.size();i++)
+ {
+ servo_ids.push_back(devices_v1[i].id);
+ data[i].clear();
+ data[i].push_back(((2000000/baudrate)-1));
+ }
+ this->write_sync(servo_ids,0x04,data,dyn_version1);
+ }
+ if(this->devices_v2.size()>0)
+ {
+ servo_ids.clear();
+ data.resize(this->devices_v2.size());
+ for(i=0;i<this->devices_v2.size();i++)
+ {
+ servo_ids.push_back(devices_v2[i].id);
+ data[i].clear();
+ data[i].push_back(((2000000/baudrate)-1));
+ }
+ this->write_sync(servo_ids,0x04,data,dyn_version2);
+ }
+ sleep(1);
+ /* reconfigure the communciations device */
+ ftdi_config.word_length=8;
+ ftdi_config.stop_bits=1;
+ ftdi_config.parity=0;
+ ftdi_config.read_timeout = 1000;
+ ftdi_config.write_timeout = 1000;
+ ftdi_config.latency_timer = 1;
+ ftdi_config.baud_rate=baudrate;
+ this->comm_dev->config(&ftdi_config);
+ this->bus_info.baud_rate=baudrate;
+ }catch(CException &e){
+ /* handle exceptions */
+ throw;
+ }
+ }
+ }
+ else
+ {
+ /* handle exceptions */
+ throw CDynamixelServerException(_HERE_,"The communication device is not ready to send information");
+ }
+}
diff --git a/src/dynamixelserver_ftdi.h b/src/dynamixelserver_ftdi.h
new file mode 100644
index 0000000000000000000000000000000000000000..8190803dbd1afc2f81a3134b4d33c52da1e12fd0
--- /dev/null
+++ b/src/dynamixelserver_ftdi.h
@@ -0,0 +1,112 @@
+#ifndef _DYNAMIXEL_SERVER_FTDI_H
+#define _DYNAMIXEL_SERVER_FTDI_H
+
+#include "dynamixelserver.h"
+
+/**
+ * \brief Basic Dynamixel bus information
+ *
+ * This structure holds the identifier of the current bus used (in case there
+ * exist multiple buses) and also the baudrate of the bus.
+ */
+typedef struct
+{
+ int baud_rate;
+ int bus_id;
+ std::string serial;
+}TBus_info;
+
+/**
+ * \brief
+ *
+ */
+class CDynamixelServerFTDI : public CDynamixelServer
+{
+ private:
+ /**
+ * \brief
+ *
+ */
+ static CDynamixelServerFTDI *pinstance;
+ /**
+ * \brief
+ *
+ */
+ TBus_info bus_info;
+ protected:
+ /**
+ * \brief
+ *
+ */
+ CDynamixelServerFTDI();
+ /**
+ * \brief
+ *
+ */
+ CDynamixelServerFTDI(const CDynamixelServerFTDI &object);
+ /**
+ * \brief
+ *
+ */
+ CDynamixelServerFTDI& operator = (const CDynamixelServerFTDI &object);
+ /**
+ * \brief
+ *
+ */
+ void set_bus_id(int bus_id);
+ /**
+ * \brief
+ *
+ */
+ void set_bus_id(std::string &bus_id);
+ public:
+ /**
+ * \brief
+ *
+ */
+ static CDynamixelServerFTDI* instance();
+ /**
+ * \brief
+ *
+ */
+ void config_bus(int bus_id,int baudrate);
+ /**
+ * \brief
+ *
+ */
+ void config_bus(std::string &bus_id,int baudrate);
+#ifdef _HAVE_XSD
+ /**
+ * \brief
+ *
+ */
+ void config(std::string &filename);
+#endif
+ /**
+ * \brief
+ *
+ */
+ void set_baudrate(int baudrate);
+ /**
+ * \brief
+ *
+ */
+ int get_num_buses(void);
+ /**
+ * \brief
+ *
+ */
+ int get_bus_id(void);
+ /**
+ * \brief
+ *
+ */
+ std::string &get_bus_serial(void);
+ /**
+ * \brief
+ *
+ */
+ int get_baudrate(void);
+};
+
+#endif
diff --git a/src/dynamixelserver_serial.cpp b/src/dynamixelserver_serial.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..94e44e09742f346004086a140026af3c1bf38132
--- /dev/null
+++ b/src/dynamixelserver_serial.cpp
@@ -0,0 +1,150 @@
+#include "dynamixelexceptions.h"
+#include "dynamixelserver_serial.h"
+#include "eventexceptions.h"
+#include "rs232.h"
+#include <sstream>
+#include <sys/types.h>
+#include <sys/stat.h>
+#include <unistd.h>
+#ifdef _HAVE_XSD
+#include "xml/dyn_server_serial_cfg_file.hxx"
+#endif
+
+CDynamixelServerSerial *CDynamixelServerSerial::pinstance=NULL;
+
+CDynamixelServerSerial::CDynamixelServerSerial()
+{
+ this->bus_info.baud_rate=-1;
+ this->bus_info.device="";
+}
+
+CDynamixelServerSerial::CDynamixelServerSerial(const CDynamixelServerSerial& object)
+{
+
+}
+
+CDynamixelServerSerial& CDynamixelServerSerial::operator = (const CDynamixelServerSerial& object)
+{
+ return *this->pinstance;
+}
+
+CDynamixelServerSerial *CDynamixelServerSerial::instance(void)
+{
+ if (CDynamixelServerSerial::pinstance == NULL)
+ {
+ CDynamixelServerSerial::pinstance = new CDynamixelServerSerial(); // Creamos la instancia
+ }
+ return CDynamixelServerSerial::pinstance; // Retornamos la dirección de la instancia
+}
+
+void CDynamixelServerSerial::config_bus(const std::string &device, int baudrate)
+{
+ TRS232_config serial_config;
+
+ serial_config.baud=baudrate;
+ serial_config.num_bits=8;
+ serial_config.parity=none;
+ serial_config.stop_bits=1;
+ this->comm_dev=new CRS232(device);
+ this->comm_dev->open((void *)&device);
+ this->comm_dev->config(&serial_config);
+ this->bus_info.baud_rate=baudrate;
+ this->bus_info.device=device;
+}
+
+#ifdef _HAVE_XSD
+void CDynamixelServerSerial::config(std::string &filename)
+{
+ struct stat buffer;
+
+ if(stat(filename.c_str(),&buffer)==0)
+ {
+ // try to open the specified file
+ try{
+ std::auto_ptr<dyn_server_serial_config_t> cfg(dyn_server_serial_config(filename.c_str(), xml_schema::flags::dont_validate));
+ this->config_bus(cfg->serial_dev(),cfg->baudrate());
+ }catch (const xml_schema::exception& e){
+ std::ostringstream os;
+ os << e;
+ /* handle exceptions */
+ throw CDynamixelServerException(_HERE_,os.str());
+ }catch(CException &e){
+ throw e;
+ }
+ }
+ else
+ throw CDynamixelServerException(_HERE_,"The configuration file does not exist");
+}
+#endif
+
+std::string &CDynamixelServerSerial::get_bus_device(void)
+{
+ return this->bus_info.device;
+}
+
+int CDynamixelServerSerial::get_baudrate(void)
+{
+ return this->bus_info.baud_rate;
+}
+
+void CDynamixelServerSerial::set_baudrate(int baudrate)
+{
+ std::vector< std::vector<unsigned char> > data;
+ std::vector<unsigned char> servo_ids;
+ TRS232_config serial_config;
+ unsigned int i=0;
+
+ if(this->comm_dev!=NULL)
+ {
+ if(baudrate <= 0 || baudrate > 1000000)
+ {
+ /* handle exceptions */
+ throw CDynamixelServerException(_HERE_,"Invalid baudrate");
+ }
+ else
+ {
+ try{
+ if(this->devices_v1.size()>0)
+ {
+ servo_ids.clear();
+ data.resize(this->devices_v1.size());
+ for(i=0;i<this->devices_v1.size();i++)
+ {
+ servo_ids.push_back(devices_v1[i].id);
+ data[i].clear();
+ data[i].push_back(((2000000/baudrate)-1));
+ }
+ this->write_sync(servo_ids,0x04,data,dyn_version1);
+ }
+ if(this->devices_v2.size()>0)
+ {
+ servo_ids.clear();
+ data.resize(this->devices_v2.size());
+ for(i=0;i<this->devices_v2.size();i++)
+ {
+ servo_ids.push_back(devices_v2[i].id);
+ data[i].clear();
+ data[i].push_back(((2000000/baudrate)-1));
+ }
+ this->write_sync(servo_ids,0x04,data,dyn_version2);
+ }
+ sleep(1);
+ /* reconfigure the communciations device */
+ serial_config.baud=baudrate;
+ serial_config.num_bits=8;
+ serial_config.parity=none;
+ serial_config.stop_bits=1;
+ this->comm_dev->config(&serial_config);
+ this->bus_info.baud_rate=baudrate;
+ }catch(CException &e){
+ /* handle exceptions */
+ throw;
+ }
+ }
+ }
+ else
+ {
+ /* handle exceptions */
+ throw CDynamixelServerException(_HERE_,"The communication device is not ready to send information");
+ }
+}
diff --git a/src/dynamixelserver_serial.h b/src/dynamixelserver_serial.h
new file mode 100644
index 0000000000000000000000000000000000000000..4a7dd2176a2f04d44b32fe1bd63112ee10db9c87
--- /dev/null
+++ b/src/dynamixelserver_serial.h
@@ -0,0 +1,86 @@
+#ifndef _DYNAMIXEL_SERVER_FTDI_H
+#define _DYNAMIXEL_SERVER_FTDI_H
+
+#include "dynamixelserver.h"
+
+/**
+ * \brief Basic Dynamixel bus information
+ *
+ * This structure holds the identifier of the current bus used (in case there
+ * exist multiple buses) and also the baudrate of the bus.
+ */
+typedef struct
+{
+ int baud_rate;
+ std::string device;
+}TBus_info;
+
+/**
+ * \brief
+ *
+ */
+class CDynamixelServerSerial : public CDynamixelServer
+{
+ private:
+ /**
+ * \brief
+ *
+ */
+ static CDynamixelServerSerial *pinstance;
+ /**
+ * \brief
+ *
+ */
+ TBus_info bus_info;
+ protected:
+ /**
+ * \brief
+ *
+ */
+ CDynamixelServerSerial();
+ /**
+ * \brief
+ *
+ */
+ CDynamixelServerSerial(const CDynamixelServerSerial &object);
+ /**
+ * \brief
+ *
+ */
+ CDynamixelServerSerial& operator = (const CDynamixelServerSerial &object);
+ public:
+ /**
+ * \brief
+ *
+ */
+ static CDynamixelServerSerial* instance();
+ /**
+ * \brief
+ *
+ */
+ void config_bus(const std::string &device,int baudrate);
+ /**
+ * \brief
+ *
+ */
+#ifdef _HAVE_XSD
+ virtual void config(std::string &filename);
+#endif
+ /**
+ * \brief
+ *
+ */
+ void set_baudrate(int baudrate);
+ /**
+ * \brief
+ *
+ */
+ std::string &get_bus_device(void);
+ /**
+ * \brief
+ *
+ */
+ int get_baudrate(void);
+};
+
+#endif
diff --git a/src/examples/CMakeLists.txt b/src/examples/CMakeLists.txt
index 84fb9f2eae1127ff64cb3ffe8ed1ef1be10bcc66..39de6f4d4188bcfbc32f28d164d53f79e6a8dba7 100644
--- a/src/examples/CMakeLists.txt
+++ b/src/examples/CMakeLists.txt
@@ -21,3 +21,9 @@ ADD_EXECUTABLE(test_dynamixel_bulk_read test_dynamixel_bulk_read.cpp)
# edit the following line to add the necessary libraries
TARGET_LINK_LIBRARIES(test_dynamixel_bulk_read dynamixel)
+
+# edit the following line to add the source code for the example and the name of the executable
+ADD_EXECUTABLE(test_new_firmware test_new_firmware.cpp)
+
+# edit the following line to add the necessary libraries
+TARGET_LINK_LIBRARIES(test_new_firmware dynamixel)
diff --git a/src/examples/test_dynamixel_bulk_read.cpp b/src/examples/test_dynamixel_bulk_read.cpp
index 9ad7c34e852f6afbde89861b076771d3fac43b73..8e457cf2f86b0aa64e70006542594be12be6ee96 100755
--- a/src/examples/test_dynamixel_bulk_read.cpp
+++ b/src/examples/test_dynamixel_bulk_read.cpp
@@ -1,11 +1,11 @@
#include "eventexceptions.h"
-#include "dynamixelserver.h"
+#include "dynamixelserver_ftdi.h"
#include "dynamixel.h"
#include <iostream>
int main(int argc, char *argv[])
{
- CDynamixelServer *dyn_server=CDynamixelServer::instance();
+ CDynamixelServerFTDI *dyn_server=CDynamixelServerFTDI::instance();
CEventServer *event_server=CEventServer::instance();
std::vector< std::vector<unsigned char> > data;
std::vector<unsigned short int> start_addr;
diff --git a/src/examples/test_dynamixel_server.cpp b/src/examples/test_dynamixel_server.cpp
index a01605b05a0f189d937223feae79cbe64178bc4b..8e3ed0f5a3e0e65793a7c54e51fc0b3910afcb24 100644
--- a/src/examples/test_dynamixel_server.cpp
+++ b/src/examples/test_dynamixel_server.cpp
@@ -1,10 +1,10 @@
#include "eventexceptions.h"
-#include "dynamixelserver.h"
+#include "dynamixelserver_ftdi.h"
#include <iostream>
int main(int argc, char *argv[])
{
- CDynamixelServer *dyn_server=CDynamixelServer::instance();
+ CDynamixelServerFTDI *dyn_server=CDynamixelServerFTDI::instance();
CEventServer *event_server=CEventServer::instance();
int num_buses=0,baudrate=0,event_id;
std::list<std::string> events;
diff --git a/src/examples/test_dynamixel_server_no_scan.cpp b/src/examples/test_dynamixel_server_no_scan.cpp
index 314ca4a5a9685316a3ab6acb35672d032e548c51..7bce23afa085beefbe154b469f5e0322b6eabf4a 100644
--- a/src/examples/test_dynamixel_server_no_scan.cpp
+++ b/src/examples/test_dynamixel_server_no_scan.cpp
@@ -1,10 +1,10 @@
#include "eventexceptions.h"
-#include "dynamixelserver.h"
+#include "dynamixelserver_ftdi.h"
#include <iostream>
int main(int argc, char *argv[])
{
- CDynamixelServer *dyn_server=CDynamixelServer::instance();
+ CDynamixelServerFTDI *dyn_server=CDynamixelServerFTDI::instance();
std::vector<int> devices;
CDynamixel *dyn_motor;
int num_buses=0;
@@ -17,31 +17,35 @@ int main(int argc, char *argv[])
try{
dyn_motor=dyn_server->get_device(13);
std::cout << "device 13 found!!!" << std::endl;
+ dyn_server->free_device(13);
}catch(CException &e){
std::cout << "device 13 not found on bus 0" << std::endl;
}
try{
dyn_motor=dyn_server->get_device(12);
std::cout << "device 12 found!!!" << std::endl;
+ dyn_server->free_device(12);
}catch(CException &e){
std::cout << "device 12 not found on bus 0" << std::endl;
}
try{
dyn_motor=dyn_server->get_device(1);
std::cout << "device 1 found!!!" << std::endl;
+ dyn_server->free_device(1);
}catch(CException &e){
std::cout << "device 1 not found on bus 0" << std::endl;
}
try{
dyn_motor=dyn_server->get_device(1);
std::cout << "device 1 found!!!" << std::endl;
+ dyn_server->free_device(1);
}catch(CException &e){
std::cout << "device 1 not found on bus 0" << std::endl;
}
- dyn_server->free_device(1);
try{
dyn_motor=dyn_server->get_device(1);
std::cout << "device 1 found!!!" << std::endl;
+ dyn_server->free_device(1);
}catch(CException &e){
std::cout << "device 1 not found on bus 0" << std::endl;
}
diff --git a/src/examples/test_dynamixel_sync.cpp b/src/examples/test_dynamixel_sync.cpp
index 778d44eddab27056b28ae8f05399292be5b78eb8..f4ab9f6f087cf86bdcc075edb3ea938ebd2b5188 100644
--- a/src/examples/test_dynamixel_sync.cpp
+++ b/src/examples/test_dynamixel_sync.cpp
@@ -1,10 +1,10 @@
#include "eventexceptions.h"
-#include "dynamixelserver.h"
+#include "dynamixelserver_ftdi.h"
#include <iostream>
int main(int argc, char *argv[])
{
- CDynamixelServer *dyn_server=CDynamixelServer::instance();
+ CDynamixelServerFTDI *dyn_server=CDynamixelServerFTDI::instance();
CEventServer *event_server=CEventServer::instance();
std::vector< std::vector<unsigned char> > data;
std::vector<unsigned char> servo_ids;
diff --git a/src/xml/CMakeLists.txt b/src/xml/CMakeLists.txt
new file mode 100644
index 0000000000000000000000000000000000000000..44ddee0ec293cadfbd7ff6fee11ed7b3c06aea85
--- /dev/null
+++ b/src/xml/CMakeLists.txt
@@ -0,0 +1,45 @@
+#check the existance of the xsd library
+IF(EXISTS "/usr/include/xsd/cxx")
+ SET(CMAKE_CXX_FLAGS_DEBUG "${CMAKE_CXX_FLAGS_DEBUG} -D_HAVE_XSD" PARENT_SCOPE)
+ SET(CMAKE_CXX_FLAGS_RELEASE "${CMAKE_CXX_FLAGS_RELEASE} -D_HAVE_XSD" PARENT_SCOPE)
+ SET(XSD_FOUND TRUE)
+ MESSAGE(STATUS "Found the XML library ... adding support for XML files")
+ FIND_LIBRARY(XSD_LIBRARY
+ NAMES xerces-c
+ PATHS /usr/lib /usr/local/lib)
+ELSE(EXISTS "/usr/include/xsd/cxx")
+ MESSAGE(STATUS "XML library not found ... it will be impossible to handle XML files")
+ENDIF(EXISTS "/usr/include/xsd/cxx")
+
+IF(XSD_FOUND)
+ SET(XSD_LIBRARY ${XSD_LIBRARY} PARENT_SCOPE)
+
+ SET(XSD_PATH ${CMAKE_CURRENT_SOURCE_DIR})
+ SET(XSD_FILES dyn_server_ftdi_cfg_file.xsd dyn_server_serial_cfg_file.xsd)
+
+ IF(XSD_FILES)
+ FOREACH(xsd_file ${XSD_FILES})
+ STRING(REGEX REPLACE "xsd" "cxx" xsd_source ${xsd_file})
+ SET(XSD_SOURCES_INT ${XSD_SOURCES_INT} ${XSD_PATH}/${xsd_source})
+ SET(XSD_SOURCES ${XSD_SOURCES} ${XSD_PATH}/${xsd_source})
+ STRING(REGEX REPLACE "xsd" "hxx" xsd_header ${xsd_file})
+ SET(XSD_HEADERS_INT ${XSD_HEADERS_INT} ${XSD_PATH}/${xsd_header})
+ SET(XSD_HEADERS ${XSD_HEADERS} ${XSD_PATH}/${xsd_header})
+ SET(XSD_PATH_FILES ${XSD_PATH_FILES} ${XSD_PATH}/${xsd_file})
+ ENDFOREACH(xsd_file)
+
+ SET(XSD_SOURCES ${XSD_SOURCES_INT} PARENT_SCOPE)
+ SET(XSD_HEADERS ${XSD_HEADERS_INT} PARENT_SCOPE)
+
+ ADD_CUSTOM_TARGET(xsd_files_gen DEPENDS ${XSD_SOURCES_INT})
+ ADD_CUSTOM_COMMAND(
+ OUTPUT ${XSD_SOURCES_INT}
+ COMMAND xsdcxx cxx-tree --generate-serialization ${XSD_FILES}
+ WORKING_DIRECTORY ${CMAKE_CURRENT_SOURCE_DIR}
+ DEPENDS ${XSD_PATH_FILES}
+ COMMENT "Parsing the xml template file ${XSD_FILES}")
+
+ INSTALL(FILES ${XSD_PATH_FILES} DESTINATION include/iridrivers/xml)
+ INSTALL(FILES ${XSD_HEADERS_INT} DESTINATION include/iridrivers/xml)
+ ENDIF(XSD_FILES)
+ENDIF(XSD_FOUND)
diff --git a/src/xml/dyn_server_ftdi_cfg_file.xsd b/src/xml/dyn_server_ftdi_cfg_file.xsd
new file mode 100755
index 0000000000000000000000000000000000000000..790ba12a63c17859c4ce3201eedf23da83401610
--- /dev/null
+++ b/src/xml/dyn_server_ftdi_cfg_file.xsd
@@ -0,0 +1,25 @@
+<?xml version="1.0"?>
+
+<!--
+
+file : dynamixel_server_ftdi.xsd
+author : Sergi Hernandez Juan (shernand@iri.upc.edu)
+copyright : not copyrighted - public domain
+
+-->
+
+<xsd:schema xmlns:xsd="http://www.w3.org/2001/XMLSchema">
+
+ <xsd:complexType name="dyn_server_ftdi_config_t">
+ <xsd:sequence>
+ <xsd:element name="baudrate" type="xsd:int">
+ </xsd:element>
+ <xsd:element name="serial_num" type="xsd:string">
+ </xsd:element>
+ </xsd:sequence>
+ </xsd:complexType>
+
+ <xsd:element name="dyn_server_ftdi_config" type="dyn_server_ftdi_config_t">
+ </xsd:element>
+
+</xsd:schema>
diff --git a/src/xml/dyn_server_serial_cfg_file.xsd b/src/xml/dyn_server_serial_cfg_file.xsd
new file mode 100755
index 0000000000000000000000000000000000000000..d544f8f3f0a9116d6c5c2843009defb3bca5821c
--- /dev/null
+++ b/src/xml/dyn_server_serial_cfg_file.xsd
@@ -0,0 +1,25 @@
+<?xml version="1.0"?>
+
+<!--
+
+file : dynamixel_server_ftdi.xsd
+author : Sergi Hernandez Juan (shernand@iri.upc.edu)
+copyright : not copyrighted - public domain
+
+-->
+
+<xsd:schema xmlns:xsd="http://www.w3.org/2001/XMLSchema">
+
+ <xsd:complexType name="dyn_server_serial_config_t">
+ <xsd:sequence>
+ <xsd:element name="baudrate" type="xsd:int">
+ </xsd:element>
+ <xsd:element name="serial_dev" type="xsd:string">
+ </xsd:element>
+ </xsd:sequence>
+ </xsd:complexType>
+
+ <xsd:element name="dyn_server_serial_config" type="dyn_server_serial_config_t">
+ </xsd:element>
+
+</xsd:schema>