Completelly integrated the GPIO to the dynamixel interface.

Added a class for general callbacks.

src/CMakeLists.txt

 # driver source files
 SET(sources bioloid_robot.cpp bioloid_robot_exceptions.cpp)
 # application header files
-SET(headers bioloid_robot.h bioloid_robot_exceptions.h)
+SET(headers bioloid_robot.h bioloid_robot_exceptions.h bioloid_robot_callbacks.h)
 # locate the necessary dependencies
 FIND_PACKAGE(iriutils REQUIRED)
 FIND_PACKAGE(comm REQUIRED)
 FIND_PACKAGE(dynamixel REQUIRED)
+FIND_PACKAGE(bbb_gpio REQUIRED)
 # add the necessary include directories
 INCLUDE_DIRECTORIES(.)
 INCLUDE_DIRECTORIES(${iriutils_INCLUDE_DIR})
 INCLUDE_DIRECTORIES(${comm_INCLUDE_DIR})
 INCLUDE_DIRECTORIES(${dynamixel_INCLUDE_DIR})
+INCLUDE_DIRECTORIES(${bbb_gpio_INCLUDE_DIR})
 INCLUDE_DIRECTORIES(~/bioloid_robot/stm32_code/include/)
 # create the shared library
 ADD_LIBRARY(bioloid_robot SHARED ${sources})
@@ -18,6 +20,8 @@ ADD_LIBRARY(bioloid_robot SHARED ${sources})
 TARGET_LINK_LIBRARIES(bioloid_robot ${iriutils_LIBRARY})
 TARGET_LINK_LIBRARIES(bioloid_robot ${comm_LIBRARY})
 TARGET_LINK_LIBRARIES(bioloid_robot ${dynamixel_LIBRARY})
+TARGET_LINK_LIBRARIES(bioloid_robot ${bbb_gpio_LIBRARY})
+
 INSTALL(TARGETS bioloid_robot
         RUNTIME DESTINATION bin
         LIBRARY DESTINATION lib/iridrivers

src/bioloid_robot.cpp

@@ -2,7 +2,7 @@
 #include "bioloid_robot_exceptions.h"
 #include <iostream>
 
-CBioloid_Robot::CBioloid_Robot(const std::string &name,std::string &serial_dev,int baudrate, unsigned char id)
+CBioloid_Robot::CBioloid_Robot(const std::string &name,std::string &serial_dev,int baudrate, unsigned char id,gpio_ports port, int pin) : ext_int(port,pin)
 {
   this->robot_device=NULL;
   this->serial_device=NULL;
@@ -22,6 +22,24 @@ CBioloid_Robot::CBioloid_Robot(const std::string &name,std::string &serial_dev,i
       this->dyn_server->config_comm(this->serial_device);
       this->robot_device=dyn_server->get_device(id);
       this->robot_id=id;
+      /* configure the gpio pin for the external interrupt */
+      this->ext_int.set_direction(input);
+      this->ext_int_event_id=this->ext_int.set_active_edge(edge_falling);
+      /* initialize internal events */
+      this->event_server=CEventServer::instance();
+      this->finish_thread_event_id=this->robot_name+"_finish_thread_event";
+      this->event_server->create_event(this->finish_thread_event_id);
+      /* initialize internal thread to handle external interrupts */
+      this->thread_server=CThreadServer::instance();
+      this->ext_int_thread_id=this->robot_name+"_thread";
+      this->thread_server->create_thread(this->ext_int_thread_id);
+      this->thread_server->attach_thread(this->ext_int_thread_id,this->ext_int_thread,this);
+      this->thread_server->start_thread(this->ext_int_thread_id);
+      /* initialize the pushbutton callbacks */
+      this->user_pb=NULL;
+      this->reset_pb=NULL;
+      this->start_pb=NULL;
+      this->mode_pb=NULL;
     }catch(CException &e){
       if(this->robot_device!=NULL)
         this->dyn_server->free_device(id);
@@ -41,6 +59,37 @@ CBioloid_Robot::CBioloid_Robot(const std::string &name,std::string &serial_dev,i
   }
 }
 
+// internal thread function
+void *CBioloid_Robot::ext_int_thread(void *param)
+{
+  CBioloid_Robot *robot=(CBioloid_Robot *)param;
+  std::list<std::string> events;
+  unsigned char value;
+  int event_id=0;
+  bool end=false;
+
+  events.push_back(robot->finish_thread_event_id);
+  events.push_back(robot->ext_int_event_id);
+
+  while(!end)
+  {
+    event_id=robot->event_server->wait_first(events);
+    if(event_id==0)
+      end=true;
+    else
+    {
+      if(event_id==1)
+      {
+        robot->robot_device->read_byte_register(BIOLOID_USER_PB_CNTRL,&value);
+        if(value&GPIO_INT_FLAG)
+          robot->user_pb->call(robot);
+      }
+    }
+  }
+
+  pthread_exit(NULL);
+}
+
 // GPIO interface
 void CBioloid_Robot::set_led(led_t led)
 {
@@ -109,10 +158,23 @@ void CBioloid_Robot::blink_led(led_t led,int period_ms)
 
 bool CBioloid_Robot::is_button_pressed(pushbutton_t pushbutton)
 {
-}
+  unsigned char value;
 
-void assign_button_callback(pushbutton_t pushbutton, void (*pb_callback)(void))
-{
+  switch(pushbutton)
+  {
+    case USER_PB: this->robot_device->read_byte_register(BIOLOID_USER_PB_CNTRL,&value);
+                  break;
+    case RESET_PB: this->robot_device->read_byte_register(BIOLOID_RESET_PB_CNTRL,&value);
+                   break;
+    case START_PB: this->robot_device->read_byte_register(BIOLOID_START_PB_CNTRL,&value);
+                   break;
+    case MODE_PB: this->robot_device->read_byte_register(BIOLOID_MODE_PB_CNTRL,&value);
+                  break;
+  }
+  if(value&GPIO_INT_FLAG)
+    return true;
+  else
+    return false;
 }
 
 // motion manager interface
@@ -329,6 +391,17 @@ bool CBioloid_Robot::action_is_page_running(void)
 
 CBioloid_Robot::~CBioloid_Robot()
 {
+  if(this->thread_server->get_thread_state(this->ext_int_thread_id)==active)
+  {
+    this->event_server->set_event(this->finish_thread_event_id);
+    this->thread_server->end_thread(this->ext_int_thread_id);
+  }
+  this->thread_server->detach_thread(this->ext_int_thread_id);
+  this->thread_server->delete_thread(this->ext_int_thread_id);
+  this->ext_int_thread_id="";
+  this->event_server->delete_event(this->finish_thread_event_id);
+  this->finish_thread_event_id="";
+
   if(this->serial_device!=NULL)
   {
     delete this->serial_device;

src/bioloid_robot.h

@@ -4,7 +4,11 @@
 #include "dynamixelserver.h"
 #include "dynamixel.h"
 #include "rs232.h"
+#include "bbb_gpio.h"
 #include "bioloid_registers.h"
+#include "bioloid_robot_callbacks.h"
+#include "threadserver.h"
+#include "eventserver.h"
 
 /* available motion modules */
 typedef enum {BIOLOD_MM_NONE=0x00,BIOLOID_MM_ACTION=0x01,BIOLOID_MM_WALKING=0x02,BIOLOID_MM_JOINTS=0x03} mm_mode_t;
@@ -23,17 +27,57 @@ class CBioloid_Robot
     /* communication interface */
     CRS232 *serial_device;
     TRS232_config serial_config;
+    /* external interrupt GPIO pin */
+    CGPIO ext_int;
+    std::string ext_int_event_id;
     /* robot's name */
     std::string robot_name;
+    /* internal thread */
+    CThreadServer *thread_server;
+    std::string ext_int_thread_id;
+    /* internal events */
+    CEventServer *event_server;
+    std::string finish_thread_event_id;
+    /* pushbutton callbacks */
+    CFunctor *user_pb;
+    CFunctor *reset_pb;
+    CFunctor *start_pb;
+    CFunctor *mode_pb;
+  protected:
+    static void *ext_int_thread(void *param);
   public:
-    CBioloid_Robot(const std::string &name,std::string &serial_dev,int baudrate, unsigned char id);
+    CBioloid_Robot(const std::string &name,std::string &serial_dev,int baudrate, unsigned char id,gpio_ports port, int pin);
     // GPIO interface
     void set_led(led_t led);
     void clear_led(led_t led);
     void toggle_led(led_t led);
     void blink_led(led_t led, int period_ms);
     bool is_button_pressed(pushbutton_t pushbutton);
-    void assign_button_callback(pushbutton_t pushbutton, void (*pb_callback)(void));
+    template<class T>
+    void assign_button_callback(pushbutton_t pushbutton,T *obj_ptr,void (T::*pb_callback)(CBioloid_Robot *))
+    {
+      switch(pushbutton)
+      {
+        case USER_PB: if(pb_callback!=NULL)
+                      {
+                        this->user_pb=new CBioloidCallback<T>(obj_ptr,pb_callback);
+                        this->robot_device->write_byte_register(BIOLOID_USER_PB_CNTRL,GPIO_INT_EN);//enable interrupts
+                      }
+                      else
+                      {
+                        delete this->user_pb;
+                        this->user_pb=NULL;
+                        this->robot_device->write_byte_register(BIOLOID_USER_PB_CNTRL,0x00);//disable interrupts
+                      }
+                      break;
+         case RESET_PB: this->robot_device->write_byte_register(BIOLOID_RESET_PB_CNTRL,GPIO_INT_EN);
+                        break;
+         case START_PB: this->robot_device->write_byte_register(BIOLOID_START_PB_CNTRL,GPIO_INT_EN);
+                        break;
+         case MODE_PB: this->robot_device->write_byte_register(BIOLOID_MODE_PB_CNTRL,GPIO_INT_EN);
+                       break;
+      }
+    }
     // motion manager interface
 /*    void mm_set_period(double period_ms);
     unsigned char mm_get_num_servos(void);

src/bioloid_robot_callbacks.h

+#ifndef _BIOLOID_ROBOT_CALLBACKS
+#define _BIOLOID_ROBOT_CALLBACKS
+
+#include "bioloid_robot_exceptions.h"
+
+class CBioloid_Robot;
+
+/**
+ * \brief 
+ *
+ */
+class CFunctor
+{
+  public:
+    /**
+     * \brief
+     * 
+     */
+    CFunctor(){};
+    /**
+     * \brief
+     *
+     */
+    virtual void call(CBioloid_Robot *robot){};
+    /**
+     * \brief 
+     *
+     */
+    virtual ~CFunctor(){};
+};
+
+
+/**
+ * \brief 
+ *
+ */
+template<class T>
+class CBioloidCallback : public CFunctor
+{   
+  private:
+    /**
+     * \irief
+     */
+    typedef void (T::*callback_fnct_ptr)(CBioloid_Robot *robot);                                         
+    /**
+     * \brief 
+     *
+     */
+    callback_fnct_ptr callback_fnct;                                                                       
+    /**
+     * \brief 
+     */
+    T *obj_ptr;                                                                                        
+  public:                                                                                              
+    /**
+     * \brief 
+     */
+    CBioloidCallback(T *obj_ptr,callback_fnct_ptr fnct_ptr)
+    { 
+     if(obj_ptr==NULL)
+      {
+        // handle exceptions
+        throw CBioloidRobotException(_HERE_,"Invalid object pointer");
+      }
+      else
+        this->obj_ptr=obj_ptr;
+      if(fnct_ptr==NULL)
+      {
+        /* handle exceptions */
+        throw CBioloidRobotException(_HERE_,"Invalid function pointer");
+      }
+      else
+        this->callback_fnct=fnct_ptr;
+    };
+    /**
+     * \brief 
+     *
+     */
+    virtual void call(CBioloid_Robot *robot)
+    {
+      (this->obj_ptr->*this->callback_fnct)(robot);
+    };
+    /**
+     * \brief 
+     */
+    virtual ~CBioloidCallback()
+    {
+      this->obj_ptr=NULL;
+      this->callback_fnct=NULL;
+    };
+};
+  
+#endif

src/examples/bioloid_robot_test.cpp

@@ -5,15 +5,58 @@
 
 std::string robot_device="/dev/ttyO1";
 
+class CCallbacks
+{
+  public:
+    CCallbacks(){};
+    void user_pb_callback(CBioloid_Robot *robot)
+    {
+      std::cout << "user push button pressed" << std::endl;
+    }
+    ~CCallbacks(){};
+};
+
 int main(int argc, char *argv[])
 {
   unsigned int i=0;
+  CCallbacks callbacks;
 
   try{
-    CBioloid_Robot tina("Tina",robot_device,921600,0x02);
+    CBioloid_Robot tina("Tina",robot_device,921600,0x02,GPIO2,25);
+/*    for(i=0;i<4;i++)
+    {
+      tina.toggle_led(RXD_LED);
+      tina.toggle_led(TXD_LED);
+      tina.toggle_led(USER1_LED);
+      tina.toggle_led(USER2_LED);
+      sleep(1);
+    }
+    tina.clear_led(RXD_LED);
+    tina.clear_led(TXD_LED);
+    tina.clear_led(USER1_LED);
+    tina.clear_led(USER2_LED);
+
+    tina.blink_led(TXD_LED,1000);
+    tina.blink_led(RXD_LED,2000);
     tina.blink_led(USER1_LED,1000);
+    tina.blink_led(USER2_LED,2000);
     sleep(10);
+
+    tina.clear_led(RXD_LED);
+    tina.clear_led(TXD_LED);
     tina.clear_led(USER1_LED);
+    tina.clear_led(USER2_LED);*/
+    tina.assign_button_callback(USER_PB,&callbacks,&CCallbacks::user_pb_callback);
+    sleep(10);
+/*    for(i=0;i<100;i++)
+    {
+      if(tina.is_button_pressed(RESET_PB))
+        std::cout << "Reset button pressed" << std::endl;
+      else
+        std::cout << "Reset button not pressed" << std::endl;
+      usleep(100000);
+    }*/
+//    tina.clear_led(USER1_LED);
 /*    for(i=0;i<10;i++)
     {
       std::cout << "toggle led" << std::endl;