diff --git a/src/CMakeLists.txt b/src/CMakeLists.txt
index 7eee5d8720b8ae9fce292ad18d90626da8904467..104a60d72e69effbd35a142abc1b618e096c136d 100644
--- a/src/CMakeLists.txt
+++ b/src/CMakeLists.txt
@@ -1,7 +1,7 @@
ADD_SUBDIRECTORY(xml)
# edit the following line to add all the source code files of the library
-SET(sources dynamixel.cpp dynamixelserver.cpp dynamixelserver_ftdi.cpp dynamixelserver_serial.cpp dynamixelexceptions.cpp)
+SET(sources dynamixel.cpp dynamixelserver.cpp dynamixelserver_ftdi.cpp dynamixelserver_serial.cpp dynamixelexceptions.cpp dynamixel_slave.cpp dynamixel_slave_ftdi.cpp)
# edit the following line to add all the header files of the library
SET(headers dynamixel.h dynamixelserver.h dynamixelserver_ftdi.h dynamixelserver_serial.h dynamixelexceptions.h dynamixel_common.h)
diff --git a/src/dynamixel_slave.cpp b/src/dynamixel_slave.cpp
index cfc0809e06de4b242cd7d84be0b11ba1e5fd480f..7773dd703c5cea0cc7e856a294a16ca743f63b33 100644
--- a/src/dynamixel_slave.cpp
+++ b/src/dynamixel_slave.cpp
@@ -1,13 +1,13 @@
#include "dynamixel_slave.h"
+#include <iostream>
-
-CDynamixelSlave::CDynamixelSlave(std::string& cont_id)
+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->evetn_server->create_event(this->new_packet_available_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->evetn_server->create_event(this->new_packet_available_event_id);
+ this->event_server->create_event(this->new_packet_available_event_id);
this->thread_server=CThreadServer::instance();
this->process_packets_thread_id=cont_id + "_thread_id";
@@ -16,28 +16,33 @@ CDynamixelSlave::CDynamixelSlave(std::string& cont_id)
this->return_delay=0;
this->return_level=return_all;
+ this->dyn_ver=dyn_ver;
this->comm_dev=NULL;
}
void *CDynamixelSlave::process_packets_thread(void *params)
{
- CDynamixelSlave *slave=(CDynamixelSlave *)param;
- int event_index,num,i,num_bytes=0;
+ CDynamixelSlave *slave=(CDynamixelSlave *)params;
std::list<std::string> events;
+ int event_index,num,i;
unsigned char *data;
bool end=false;
+
+ static bool data_phase=false;
+ static unsigned char packet[256];
+ static int num_bytes=0,length=0;
// wait until the comm device becomes valid
while(!end)
{
slave->comm_access.enter();
- if(slave->comm!=NULL)
+ if(slave->comm_dev!=NULL)
end=true;
slave->comm_access.exit();
}
end=false;
- events.push_back(slave->comm->get_rx_event_id());
+ events.push_back(slave->comm_dev->get_rx_event_id());
events.push_back(slave->new_packet_available_event_id);
while(!end)
{
@@ -47,81 +52,92 @@ void *CDynamixelSlave::process_packets_thread(void *params)
else
{
// process the incomming data
- num=slave->comm->get_num_data();
+ num=slave->comm_dev->get_num_data();
data=new unsigned char[num];
- if(slave->comm->read(data,num)!=num)
+ if(slave->comm_dev->read(data,num)!=num)
std::cout << "Error while reading the communication device" << std::endl;
else
{
for(i=0;i<num;i++)
{
- switch(num_bytes)
- {
- case 0: if(data[i]==0xFF)
- {
- dyn->rx_buffer[dyn->received_bytes]=byte;
- dyn->received_bytes++;
- }
- break;
- case 1: if(byte==0xFF)
- {
- dyn->rx_buffer[dyn->received_bytes]=byte;
- dyn->received_bytes++;
- }
- else
- dyn->received_bytes--;
- break;
- case 2: if(byte==0xFD)// version 2 header
- {
- if(dyn->version==DYN_VER2)// the module is configured for version 2
+ if(!data_phase)
+ {
+ switch(num_bytes)
+ {
+ case 0: if(data[i]==0xFF)
+ {
+ packet[num_bytes]=data[i];
+ num_bytes++;
+ }
+ break;
+ case 1: if(data[i]==0xFF)
{
- dyn->rx_buffer[dyn->received_bytes]=byte;
- dyn->received_bytes++;
+ packet[num_bytes]=data[i];
+ num_bytes++;
}
else
- dyn->received_bytes=0;// ignore packet and restart synchronization
- }
- else if(byte!=0xFF)
- {
- dyn->rx_buffer[dyn->received_bytes]=byte;
- dyn->received_bytes++;
- }
- break;
- case 3: dyn->rx_buffer[dyn->received_bytes]=byte;
- if(dyn->version==DYN_VER1)
- {
- dyn->op_length=byte;
- dyn->received_bytes=0;
- /* finish reception by IRQ */
- comm_cancel_irq_receive(dyn->comm_dev);
- /* enable dma RX */
- comm_receive_dma(dyn->comm_dev,&dyn->rx_buffer[4],dyn->op_length);
- }
- else
- dyn->received_bytes++;
- break;
- case 4: dyn->rx_buffer[dyn->received_bytes]=byte;
- dyn->received_bytes++;
- break;
- case 5: dyn->rx_buffer[dyn->received_bytes]=byte;
- dyn->received_bytes++;
- dyn->op_length=byte;
- break;
- case 6: dyn->rx_buffer[dyn->received_bytes]=byte;
- dyn->received_bytes++;
- dyn->op_length+=(byte<<8);
- dyn->received_bytes=0;
- /* finish reception by IRQ */
- comm_cancel_irq_receive(dyn->comm_dev);
- /* enable dma RX */
- comm_receive_dma(dyn->comm_dev,&dyn->rx_buffer[7],dyn->op_length);
- break;
- default: break;
- }
-
+ 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);
+ num_bytes++;
+ /* read packet_data */
+ data_phase=true;
+ break;
+ default: break;
+ }
+ }
+ else// data phase
+ {
+ packet[num_bytes]=data[i];
+ num_bytes++;
+ length--;
+ if(length==0)
+ {
+ num_bytes=0;
+ data_phase=false;
+ std::cout << "new packet" << std::endl;
+ }
+ }
}
}
}
+ delete[] data;
}
pthread_exit(NULL);
@@ -132,13 +148,13 @@ void CDynamixelSlave::handle_error(unsigned char error)
}
-void CDynamixelServer::start(void)
+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);
}
-void CDynamixelServer::stop(void)
+void CDynamixelSlave::stop(void)
{
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)
{
diff --git a/src/dynamixel_slave.h b/src/dynamixel_slave.h
index 7a47cd176db4272dd7c75d4005ea4d2736fe8fe4..042a28c97b915c0a0068b293879814890f52a235 100644
--- a/src/dynamixel_slave.h
+++ b/src/dynamixel_slave.h
@@ -3,7 +3,7 @@
#include "dynamixel_common.h"
#include "eventserver.h"
-#include "thradserver.h"
+#include "threadserver.h"
#include "comm.h"
#include "mutex.h"
#include <stdio.h>
@@ -18,11 +18,6 @@
class CDynamixelSlave
{
private:
- /**
- * \brief mutual exclusion mechanism to access the usb
- *
- */
- CMutex comm_access;
/**
* \brief Handle to the unique event server
*
@@ -63,7 +58,17 @@ class CDynamixelSlave
*
*/
return_level_t return_level;
+ /**
+ * \brief
+ *
+ */
+ dyn_version_t dyn_ver;
protected:
+ /**
+ * \brief mutual exclusion mechanism to access the usb
+ *
+ */
+ CMutex comm_access;
/**
* \brief Handle to the communications device
*
@@ -94,7 +99,7 @@ class CDynamixelSlave
* \brief
*
*/
- CDynamixelSlave(std::string& cont_id);
+ CDynamixelSlave(const std::string& cont_id,dyn_version_t dyn_ver=dyn_version1);
/**
* \brief
*
diff --git a/src/dynamixel_slave_ftdi.cpp b/src/dynamixel_slave_ftdi.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..47335a99309c9a5847447408f9a5e10982dbd39c
--- /dev/null
+++ b/src/dynamixel_slave_ftdi.cpp
@@ -0,0 +1,238 @@
+#include "dynamixelexceptions.h"
+#include "dynamixel_slave_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
+
+CDynamixelSlaveFTDI::CDynamixelSlaveFTDI(const std::string& cont_id,dyn_version_t dyn_ver):CDynamixelSlave(cont_id,dyn_ver)
+{
+ this->bus_info.baud_rate=-1;
+ this->bus_info.bus_id=-1;
+ this->bus_info.serial="";
+}
+
+void CDynamixelSlaveFTDI::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;
+ /* start the operation of the slave */
+ this->start();
+}
+
+void CDynamixelSlaveFTDI::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;
+ /* start the operation of the slave */
+ this->start();
+}
+
+#ifdef _HAVE_XSD
+void CDynamixelSlaveFTDI::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 CDynamixelSlaveFTDI::get_num_buses(void)
+{
+ CFTDIServer *ftdi_server=CFTDIServer::instance();
+ int num_dev=0,i=0,num_buses=0;
+ std::string description;
+
+ this->comm_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->comm_access.exit();
+ throw;
+ }
+ this->comm_access.exit();
+
+ return num_buses;
+}
+
+int CDynamixelSlaveFTDI::get_bus_id(void)
+{
+ return this->bus_info.bus_id;
+}
+
+std::string &CDynamixelSlaveFTDI::get_bus_serial(void)
+{
+ return this->bus_info.serial;
+}
+
+void CDynamixelSlaveFTDI::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 CDynamixelSlaveFTDI::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 CDynamixelSlaveFTDI::get_baudrate(void)
+{
+ int baud_rate=0;
+
+ this->comm_access.enter();
+ baud_rate=this->bus_info.baud_rate;
+ this->comm_access.exit();
+
+ return baud_rate;
+}
+
+void CDynamixelSlaveFTDI::set_baudrate(int baudrate)
+{
+ std::vector< std::vector<unsigned char> > data;
+ std::vector<unsigned char> servo_ids;
+ TFTDIconfig ftdi_config;
+
+ if(this->comm_dev!=NULL)
+ {
+ if(baudrate <= 0 || baudrate > 1000000)
+ {
+ /* handle exceptions */
+ throw CDynamixelServerException(_HERE_,"Invalid baudrate");
+ }
+ else
+ {
+ try{
+ /* 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");
+ }
+}
+
+CDynamixelSlaveFTDI::~CDynamixelSlaveFTDI()
+{
+ this->stop();
+}
diff --git a/src/dynamixel_slave_ftdi.h b/src/dynamixel_slave_ftdi.h
new file mode 100644
index 0000000000000000000000000000000000000000..08f9f4203907d76b133d8bf119d5f379d5f40eec
--- /dev/null
+++ b/src/dynamixel_slave_ftdi.h
@@ -0,0 +1,97 @@
+#ifndef _DYNAMIXEL_SLAVE_FTDI_H
+#define _DYNAMIXEL_SLAVE_FTDI_H
+
+#include "dynamixel_slave.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 CDynamixelSlaveFTDI : public CDynamixelSlave
+{
+ private:
+ /**
+ * \brief
+ *
+ */
+ TBus_info bus_info;
+ protected:
+ /**
+ * \brief
+ *
+ */
+ void set_bus_id(int bus_id);
+ /**
+ * \brief
+ *
+ */
+ void set_bus_id(std::string &bus_id);
+ public:
+ /**
+ * \brief
+ *
+ */
+ CDynamixelSlaveFTDI(const std::string& cont_id,dyn_version_t dyn_ver=dyn_version1);
+ /**
+ * \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);
+ /**
+ * \brief
+ *
+ */
+ virtual ~CDynamixelSlaveFTDI();
+};
+
+#endif
diff --git a/src/examples/CMakeLists.txt b/src/examples/CMakeLists.txt
index 84fb9f2eae1127ff64cb3ffe8ed1ef1be10bcc66..d882fdcd05bcada8f742964d66b900afc9937316 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_dynamixel_slave test_dynamixel_slave.cpp)
+
+# edit the following line to add the necessary libraries
+TARGET_LINK_LIBRARIES(test_dynamixel_slave dynamixel)
diff --git a/src/examples/test_dynamixel_slave.cpp b/src/examples/test_dynamixel_slave.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..bf8d8d5aa0e7f62b861740f60f6156b95918ab48
--- /dev/null
+++ b/src/examples/test_dynamixel_slave.cpp
@@ -0,0 +1,22 @@
+#include "eventexceptions.h"
+#include "dynamixel_slave_ftdi.h"
+#include <iostream>
+
+int main(int argc, char *argv[])
+{
+ CDynamixelSlaveFTDI slave("slave");
+
+ try{
+ slave.config_bus(0,1000000);
+ sleep(10);
+ }catch(CException &e){
+ std::cout << e.what() << std::endl;
+ }
+}
+
+
+
+
+
+
+