From 076acb0f56b735bf245a70b2a255391a6d95c63b Mon Sep 17 00:00:00 2001
From: =?UTF-8?q?Sergi=20Hern=C3=A0ndez=20Juan?= <shernand@iri.upc.edu>
Date: Thu, 17 Dec 2015 07:21:34 +0000
Subject: [PATCH] Added a Dynamixel slave modules. Work in progress.
---
src/dynamixel.h | 18 +--
src/dynamixel_common.h | 23 ++++
src/dynamixel_slave.cpp | 286 ++++++++++++++++++++++++++++++++++++++++
src/dynamixel_slave.h | 234 ++++++++++++++++++++++++++++++++
4 files changed, 544 insertions(+), 17 deletions(-)
create mode 100644 src/dynamixel_common.h
create mode 100644 src/dynamixel_slave.cpp
create mode 100644 src/dynamixel_slave.h
diff --git a/src/dynamixel.h b/src/dynamixel.h
index a361d6a..eebb8cf 100644
--- a/src/dynamixel.h
+++ b/src/dynamixel.h
@@ -2,6 +2,7 @@
#define _DYNAMIXEL_H
//#include "dynamixelserver.h"
+#include "dynamixel_common.h"
#include "eventserver.h"
#include "comm.h"
#include "mutex.h"
@@ -14,23 +15,6 @@ class CDynamixelServer;
const int NUM_RETRIES=10;
-typedef enum {dyn_version1=1,dyn_version2=2} dyn_version_t;
-
-typedef enum {dyn_ping=0x01,
- dyn_read=0x02,
- dyn_write=0x03,
- dyn_reg_write=0x04,
- dyn_action=0x05,
- dyn_factory_reset=0x06,
- dyn_reboot=0x08,
- dyn_status=0x55,
- dyn_sync_read=0x82,
- dyn_sync_write=0x83,
- dyn_bulk_read=0x92,
- dyn_bulk_write=0x93} dyn_inst_t;
-
-typedef enum {dyn_reset_all=0xFF,dyn_reset_keep_id=0x01,dyn_reset_keep_id_baud=0x02} dyn_reset_mode_t;
-
/**
* \brief
*
diff --git a/src/dynamixel_common.h b/src/dynamixel_common.h
new file mode 100644
index 0000000..b1223bc
--- /dev/null
+++ b/src/dynamixel_common.h
@@ -0,0 +1,23 @@
+#ifndef _DYNAMIXEL_COMMON_H
+#define _DYNAMIXEL_COMMON_H
+
+typedef enum {dyn_version1=1,dyn_version2=2} dyn_version_t;
+
+typedef enum {dyn_ping=0x01,
+ dyn_read=0x02,
+ dyn_write=0x03,
+ dyn_reg_write=0x04,
+ dyn_action=0x05,
+ dyn_factory_reset=0x06,
+ dyn_reboot=0x08,
+ dyn_status=0x55,
+ dyn_sync_read=0x82,
+ dyn_sync_write=0x83,
+ dyn_bulk_read=0x92,
+ dyn_bulk_write=0x93} dyn_inst_t;
+
+typedef enum {dyn_reset_all=0xFF,dyn_reset_keep_id=0x01,dyn_reset_keep_id_baud=0x02} dyn_reset_mode_t;
+
+typedef enum{no_return=0x00,return_only_read=0x01,return_all=0x02} return_level_t;
+
+#endif
diff --git a/src/dynamixel_slave.cpp b/src/dynamixel_slave.cpp
new file mode 100644
index 0000000..cfc0809
--- /dev/null
+++ b/src/dynamixel_slave.cpp
@@ -0,0 +1,286 @@
+#include "dynamixel_slave.h"
+
+
+CDynamixelSlave::CDynamixelSlave(std::string& cont_id)
+{
+ 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->finish_thread_event_id=cont_id + "finish_thread_event_id";
+ this->evetn_server->create_event(this->new_packet_available_event_id);
+
+ this->thread_server=CThreadServer::instance();
+ this->process_packets_thread_id=cont_id + "_thread_id";
+ this->thread_server->create_thread(this->process_packets_thread_id);
+ this->thread_server->attach_thread(this->process_packets_thread_id,this->process_packets_thread,this);
+
+ this->return_delay=0;
+ this->return_level=return_all;
+
+ this->comm_dev=NULL;
+}
+
+void *CDynamixelSlave::process_packets_thread(void *params)
+{
+ CDynamixelSlave *slave=(CDynamixelSlave *)param;
+ int event_index,num,i,num_bytes=0;
+ std::list<std::string> events;
+ unsigned char *data;
+ bool end=false;
+
+ // wait until the comm device becomes valid
+ while(!end)
+ {
+ slave->comm_access.enter();
+ if(slave->comm!=NULL)
+ end=true;
+ slave->comm_access.exit();
+ }
+ end=false;
+ events.push_back(slave->comm->get_rx_event_id());
+ events.push_back(slave->new_packet_available_event_id);
+ while(!end)
+ {
+ event_index=slave->event_server->wait_first(events);
+ if(event_index==0)
+ end=true;
+ else
+ {
+ // process the incomming data
+ num=slave->comm->get_num_data();
+ data=new unsigned char[num];
+ if(slave->comm->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
+ {
+ dyn->rx_buffer[dyn->received_bytes]=byte;
+ dyn->received_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;
+ }
+
+ }
+ }
+ }
+ }
+
+ pthread_exit(NULL);
+}
+
+void CDynamixelSlave::handle_error(unsigned char error)
+{
+
+}
+
+void CDynamixelServer::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)
+{
+ 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)
+ {
+ this->event_server->set_event(this->finish_thread_event_id);
+ this->thread_server->end_thread(this->process_packets_thread_id);
+ }
+}
+
+std::string CDynamixelSlave::get_new_packet_available_event_id(void)
+{
+ return this->new_packet_available_event_id;
+}
+
+void CDynamixelSlave::set_return_delay(unsigned int time_us)
+{
+ if(time_us<1000)
+ this->return_delay=time_us;
+ else
+ {
+ /* handle exceptions */
+ }
+}
+
+unsigned int CDynamixelSlave::get_return_delay(void)
+{
+ return this->return_delay;
+}
+
+void CDynamixelSlave::set_return_level(return_level_t level)
+{
+ this->return_level=level;
+}
+
+return_level_t CDynamixelSlave::get_return_level(void)
+{
+ return this->return_level;
+}
+
+bool CDynamixelSlave::new_packet_available(void)
+{
+ if(this->event_server->event_is_set(this->new_packet_available_event_id))
+ return true;
+ else
+ return false;
+}
+
+unsigned char CDynamixelSlave::get_target_id(void)
+{
+
+}
+
+dyn_inst_t CDynamixelSlave::get_instruction_type(void)
+{
+
+}
+
+// instruction specific functions
+/* read instruction */
+unsigned short int CDynamixelSlave::get_read_length(void)
+{
+
+}
+
+unsigned short int CDynamixelSlave::get_read_address(void)
+{
+
+}
+
+/* write instruction */
+unsigned short int CDynamixelSlave::get_write_address(void)
+{
+
+}
+
+unsigned short int CDynamixelSlave::get_write_length(void)
+{
+
+}
+
+void CDynamixelSlave::get_write_data(std::vector<unsigned char> &data)
+{
+
+}
+
+/* registered write instruction */
+unsigned short int CDynamixelSlave::get_reg_write_address(void)
+{
+
+}
+
+unsigned short int CDynamixelSlave::get_reg_write_length(void)
+{
+
+}
+
+void CDynamixelSlave::get_reg_write_data(std::vector<unsigned char> &data)
+{
+
+}
+
+/* sync write instruction */
+bool CDynamixelSlave::sync_write_id_present(unsigned char id,unsigned short int *address,unsigned short int *length,std::vector<unsigned char> &data)
+{
+
+}
+
+/* sync read instruction */
+bool CDynamixelSlave::sync_read_id_present(unsigned char id,unsigned short int *address,unsigned short int *length)
+{
+
+}
+
+/* bulk read instruction */
+bool CDynamixelSlave::bulk_read_id_present(unsigned char id,std::vector<unsigned short int> &address,std::vector<unsigned short int> &length)
+{
+
+}
+
+/* bulk write instruction */
+bool CDynamixelSlave::bulk_write_id_present(unsigned char id,std::vector<unsigned short int> &address,std::vector<unsigned short int> &length,std::vector<unsigned char> &data)
+{
+
+}
+
+// status return
+void CDynamixelSlave::send_status_packet(unsigned char id,TDynError error,unsigned short int length,std::vector<unsigned char> &data)
+{
+
+}
+
+void CDynamixelSlave::next_packet(void)
+{
+
+}
+CDynamixelSlave::~CDynamixelSlave()
+{
+ this->thread_server->detach_thread(this->process_packets_thread_id);
+ this->thread_server->delete_thread(this->process_packets_thread_id);
+ this->process_packets_thread_id="";
+
+ this->event_server->delete_event(this->finish_thread_event_id);
+ this->finish_thread_event_id="";
+ this->event_server->delete_event(this->new_packet_available_event_id);
+ this->new_packet_available_event_id="";
+}
diff --git a/src/dynamixel_slave.h b/src/dynamixel_slave.h
new file mode 100644
index 0000000..7a47cd1
--- /dev/null
+++ b/src/dynamixel_slave.h
@@ -0,0 +1,234 @@
+#ifndef _DYNAMIXEL_SLAVE_H
+#define _DYNAMIXEL_SLAVE_H
+
+#include "dynamixel_common.h"
+#include "eventserver.h"
+#include "thradserver.h"
+#include "comm.h"
+#include "mutex.h"
+#include <stdio.h>
+#include <stdlib.h>
+#include <string>
+#include <vector>
+
+/**
+ * \brief
+ *
+ */
+class CDynamixelSlave
+{
+ private:
+ /**
+ * \brief mutual exclusion mechanism to access the usb
+ *
+ */
+ CMutex comm_access;
+ /**
+ * \brief Handle to the unique event server
+ *
+ */
+ CEventServer *event_server;
+ /**
+ * \brief
+ *
+ */
+ std::string new_packet_available_event_id;
+ /**
+ * \brief
+ *
+ */
+ std::string finish_thread_event_id;
+ /**
+ * \brief
+ *
+ */
+ CThreadServer *thread_server;
+ /**
+ * \brief
+ *
+ */
+ std::string process_packets_thread_id;
+ /**
+ * \brief
+ *
+ */
+ dyn_version_t version;
+ /**
+ * \brief
+ *
+ */
+ unsigned int return_delay;
+ /**
+ * \brief
+ *
+ */
+ return_level_t return_level;
+ protected:
+ /**
+ * \brief Handle to the communications device
+ *
+ */
+ CComm *comm_dev;
+ /**
+ * \brief
+ *
+ */
+ static void *process_packets_thread(void *params);
+ /**
+ * \brief
+ *
+ */
+ void handle_error(unsigned char error);
+ /**
+ * \brief
+ *
+ */
+ void start(void);
+ /**
+ * \brief
+ *
+ */
+ void stop(void);
+ public:
+ /**
+ * \brief
+ *
+ */
+ CDynamixelSlave(std::string& cont_id);
+ /**
+ * \brief
+ *
+ */
+ std::string get_new_packet_available_event_id(void);
+ /**
+ * \brief
+ *
+ */
+ virtual void set_baudrate(int baudrate)=0;
+ /**
+ * \brief
+ *
+ */
+ virtual int get_baudrate(void)=0;
+ /**
+ * \brief
+ *
+ */
+ void set_return_delay(unsigned int time_us);
+ /**
+ * \brief
+ *
+ */
+ unsigned int get_return_delay(void);
+ /**
+ * \brief
+ *
+ */
+ void set_return_level(return_level_t level);
+ /**
+ * \brief
+ *
+ */
+ return_level_t get_return_level(void);
+ /**
+ * \brief
+ *
+ */
+ bool new_packet_available(void);
+ /**
+ * \brief
+ *
+ */
+ unsigned char get_target_id(void);
+ /**
+ * \brief
+ *
+ */
+ dyn_inst_t get_instruction_type(void);
+ // instruction specific functions
+ /* read instruction */
+ /**
+ * \brief
+ *
+ */
+ unsigned short int get_read_length(void);
+ /**
+ * \brief
+ *
+ */
+ unsigned short int get_read_address(void);
+ /* write instruction */
+ /**
+ * \brief
+ *
+ */
+ unsigned short int get_write_address(void);
+ /**
+ * \brief
+ *
+ */
+ unsigned short int get_write_length(void);
+ /**
+ * \brief
+ *
+ */
+ void get_write_data(std::vector<unsigned char> &data);
+ /* registered write instruction */
+ /**
+ * \brief
+ *
+ */
+ unsigned short int get_reg_write_address(void);
+ /**
+ * \brief
+ *
+ */
+ unsigned short int get_reg_write_length(void);
+ /**
+ * \brief
+ *
+ */
+ void get_reg_write_data(std::vector<unsigned char> &data);
+ /* sync write instruction */
+ /**
+ * \brief
+ *
+ */
+ bool sync_write_id_present(unsigned char id,unsigned short int *address,unsigned short int *length,std::vector<unsigned char> &data);
+ /* sync read instruction */
+ /**
+ * \brief
+ *
+ */
+ bool sync_read_id_present(unsigned char id,unsigned short int *address,unsigned short int *length);
+ /* bulk read instruction */
+ /**
+ * \brief
+ *
+ */
+ bool bulk_read_id_present(unsigned char id,std::vector<unsigned short int> &address,std::vector<unsigned short int> &length);
+ /* bulk write instruction */
+ /**
+ * \brief
+ *
+ */
+ bool bulk_write_id_present(unsigned char id,std::vector<unsigned short int> &address,std::vector<unsigned short int> &length,std::vector<unsigned char> &data);
+ // status return
+ /**
+ * \brief
+ *
+ */
+ void send_status_packet(unsigned char id,TDynError error,unsigned short int length,std::vector<unsigned char> &data);
+ /**
+ * \brief
+ *
+ */
+ void next_packet(void);
+ /**
+ * \brief
+ *
+ */
+ virtual ~CDynamixelSlave();
+};
+
+#endif
--
GitLab