Added a Dynamixel slave modules. Work in progress.

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 
  *

src/dynamixel_common.h

+#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

src/dynamixel_slave.cpp

+#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="";
+}

src/dynamixel_slave.h

+#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