From 059a11bf870ae4cf26dfbc5ec95a72b3c46c5363 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Sergi=20Hern=C3=A0ndez=20Juan?= <shernand@iri.upc.edu> Date: Wed, 20 Jan 2016 17:30:04 +0000 Subject: [PATCH] Added the FTDI implementation of the dynamixel slave. Solved some minor bugs. --- src/CMakeLists.txt | 2 +- src/dynamixel_slave.cpp | 162 ++++++++++-------- src/dynamixel_slave.h | 19 +- src/dynamixel_slave_ftdi.cpp | 238 ++++++++++++++++++++++++++ src/dynamixel_slave_ftdi.h | 97 +++++++++++ src/examples/CMakeLists.txt | 6 + src/examples/test_dynamixel_slave.cpp | 22 +++ 7 files changed, 465 insertions(+), 81 deletions(-) create mode 100644 src/dynamixel_slave_ftdi.cpp create mode 100644 src/dynamixel_slave_ftdi.h create mode 100644 src/examples/test_dynamixel_slave.cpp diff --git a/src/CMakeLists.txt b/src/CMakeLists.txt index 7eee5d8..104a60d 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 cfc0809..7773dd7 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 7a47cd1..042a28c 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 0000000..47335a9 --- /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 0000000..08f9f42 --- /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 84fb9f2..d882fdc 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 0000000..bf8d8d5 --- /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; + } +} + + + + + + + -- GitLab