Changed the name of the main class.

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,gpio_ports port, int pin) : ext_int(port,pin)
+CBioloidRobot::CBioloidRobot(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;
@@ -40,6 +40,7 @@ CBioloid_Robot::CBioloid_Robot(const std::string &name,std::string &serial_dev,i
       this->reset_pb=NULL;
       this->start_pb=NULL;
       this->mode_pb=NULL;
+      this->clear_pending_interrupts();
     }catch(CException &e){
       if(this->robot_device!=NULL)
         this->dyn_server->free_device(id);
@@ -60,9 +61,9 @@ CBioloid_Robot::CBioloid_Robot(const std::string &name,std::string &serial_dev,i
 }
 
 // internal thread function
-void *CBioloid_Robot::ext_int_thread(void *param)
+void *CBioloidRobot::ext_int_thread(void *param)
 {
-  CBioloid_Robot *robot=(CBioloid_Robot *)param;
+  CBioloidRobot *robot=(CBioloidRobot *)param;
   std::list<std::string> events;
   unsigned char value;
   int event_id=0;
@@ -83,6 +84,15 @@ void *CBioloid_Robot::ext_int_thread(void *param)
         robot->robot_device->read_byte_register(BIOLOID_USER_PB_CNTRL,&value);
         if(value&GPIO_INT_FLAG)
           robot->user_pb->call(robot);
+        robot->robot_device->read_byte_register(BIOLOID_RESET_PB_CNTRL,&value);
+        if(value&GPIO_INT_FLAG)
+          robot->reset_pb->call(robot);
+        robot->robot_device->read_byte_register(BIOLOID_START_PB_CNTRL,&value);
+        if(value&GPIO_INT_FLAG)
+          robot->start_pb->call(robot);
+        robot->robot_device->read_byte_register(BIOLOID_MODE_PB_CNTRL,&value);
+        if(value&GPIO_INT_FLAG)
+          robot->mode_pb->call(robot);
       }
     }
   }
@@ -90,8 +100,18 @@ void *CBioloid_Robot::ext_int_thread(void *param)
   pthread_exit(NULL);
 }
 
+void CBioloidRobot::clear_pending_interrupts(void)
+{
+  unsigned char value;
+
+  this->robot_device->read_byte_register(BIOLOID_USER_PB_CNTRL,&value);
+  this->robot_device->read_byte_register(BIOLOID_RESET_PB_CNTRL,&value);
+  this->robot_device->read_byte_register(BIOLOID_START_PB_CNTRL,&value);
+  this->robot_device->read_byte_register(BIOLOID_MODE_PB_CNTRL,&value);
+}
+
 // GPIO interface
-void CBioloid_Robot::set_led(led_t led)
+void CBioloidRobot::set_led(led_t led)
 {
   switch(led)
   {
@@ -106,7 +126,7 @@ void CBioloid_Robot::set_led(led_t led)
   }
 }
 
-void CBioloid_Robot::clear_led(led_t led)
+void CBioloidRobot::clear_led(led_t led)
 {
   switch(led)
   {
@@ -121,7 +141,7 @@ void CBioloid_Robot::clear_led(led_t led)
   }
 }
 
-void CBioloid_Robot::toggle_led(led_t led)
+void CBioloidRobot::toggle_led(led_t led)
 {
   switch(led)
   {
@@ -136,7 +156,7 @@ void CBioloid_Robot::toggle_led(led_t led)
   }
 }
 
-void CBioloid_Robot::blink_led(led_t led,int period_ms)
+void CBioloidRobot::blink_led(led_t led,int period_ms)
 {
   unsigned char data[3];
 
@@ -156,7 +176,7 @@ void CBioloid_Robot::blink_led(led_t led,int period_ms)
   }
 }
 
-bool CBioloid_Robot::is_button_pressed(pushbutton_t pushbutton)
+bool CBioloidRobot::is_button_pressed(pushbutton_t pushbutton)
 {
   unsigned char value;
 
@@ -178,7 +198,7 @@ bool CBioloid_Robot::is_button_pressed(pushbutton_t pushbutton)
 }
 
 // motion manager interface
-/*void CBioloid_Robot::mm_set_period(double period_ms)
+/*void CBioloidRobot::mm_set_period(double period_ms)
 {
   unsigned short period;
   // internal time in 0|16 fixed point float format in seconds
@@ -186,34 +206,34 @@ bool CBioloid_Robot::is_button_pressed(pushbutton_t pushbutton)
   this->robot_device->write_word_register(BIOLOID_MM_PERIOD_LOW,period);
 }
 
-unsigned char CBioloid_Robot::mm_get_num_servos(void)
+unsigned char CBioloidRobot::mm_get_num_servos(void)
 {
   this->robot_device->read_byte_register(BIOLOID_MM_NUM_SERVOS,&this->num_servos);
 
   return this->num_servos;
 }
 
-void CBioloid_Robot::mm_start(void)
+void CBioloidRobot::mm_start(void)
 {
   this->robot_device->write_byte_register(BIOLOID_MM_CONTROL,0x01);
 }
 
-void CBioloid_Robot::mm_stop(void)
+void CBioloidRobot::mm_stop(void)
 {
   this->robot_device->write_byte_register(BIOLOID_MM_CONTROL,0x00);
 }
 
-void CBioloid_Robot::mm_enable_power(void)
+void CBioloidRobot::mm_enable_power(void)
 {
   this->robot_device->write_byte_register(BIOLOID_DXL_POWER,0x01);
 }
 
-void CBioloid_Robot::mm_disable_power(void)
+void CBioloidRobot::mm_disable_power(void)
 {
   this->robot_device->write_byte_register(BIOLOID_DXL_POWER,0x00);
 }
 
-bool CBioloid_Robot::mm_is_power_enabled(void)
+bool CBioloidRobot::mm_is_power_enabled(void)
 {
   unsigned char value;
 
@@ -224,7 +244,7 @@ bool CBioloid_Robot::mm_is_power_enabled(void)
     return false;
 }
 
-void CBioloid_Robot::mm_enable_servo(unsigned char servo_id)
+void CBioloidRobot::mm_enable_servo(unsigned char servo_id)
 {
   unsigned char value;
 
@@ -241,7 +261,7 @@ void CBioloid_Robot::mm_enable_servo(unsigned char servo_id)
   this->robot_device->write_byte_register(BIOLOID_MM_MODULE_EN0+servo_id/2,value);
 }
 
-void CBioloid_Robot::mm_disable_servo(unsigned char servo_id)
+void CBioloidRobot::mm_disable_servo(unsigned char servo_id)
 {
   unsigned char value;
 
@@ -258,7 +278,7 @@ void CBioloid_Robot::mm_disable_servo(unsigned char servo_id)
   this->robot_device->write_byte_register(BIOLOID_MM_MODULE_EN0+servo_id/2,value);
 }
 
-bool CBioloid_Robot::mm_is_servo_enabled(unsigned char servo_id)
+bool CBioloidRobot::mm_is_servo_enabled(unsigned char servo_id)
 {
   unsigned char value;
 
@@ -284,7 +304,7 @@ bool CBioloid_Robot::mm_is_servo_enabled(unsigned char servo_id)
   }
 }
 
-void CBioloid_Robot::mm_assign_module(unsigned char servo_id, mm_mode_t mode)
+void CBioloidRobot::mm_assign_module(unsigned char servo_id, mm_mode_t mode)
 {
   unsigned char value;
 
@@ -307,7 +327,7 @@ void CBioloid_Robot::mm_assign_module(unsigned char servo_id, mm_mode_t mode)
   this->robot_device->write_byte_register(BIOLOID_MM_MODULE_EN0+servo_id/2,value);
 }
 
-mm_mode_t CBioloid_Robot::mm_get_module(unsigned char servo_id)
+mm_mode_t CBioloidRobot::mm_get_module(unsigned char servo_id)
 {
   unsigned char value;
 
@@ -323,7 +343,7 @@ mm_mode_t CBioloid_Robot::mm_get_module(unsigned char servo_id)
     return (mm_mode_t)((value&0x70)>>4);
 }
 
-std::vector<double> CBioloid_Robot::mm_get_servo_angles(void)
+std::vector<double> CBioloidRobot::mm_get_servo_angles(void)
 {
   int i=0;
   short int values[MAX_NUM_SERVOS]; 
@@ -337,7 +357,7 @@ std::vector<double> CBioloid_Robot::mm_get_servo_angles(void)
   return angles;
 }
 
-double CBioloid_Robot::mm_get_servo_angle(unsigned char servo_id)
+double CBioloidRobot::mm_get_servo_angle(unsigned char servo_id)
 {
   unsigned short int value;
   double angle;
@@ -353,12 +373,12 @@ double CBioloid_Robot::mm_get_servo_angle(unsigned char servo_id)
 }*/
 
 // motion action interface
-/*void CBioloid_Robot::action_load_page(unsigned char page_id)
+/*void CBioloidRobot::action_load_page(unsigned char page_id)
 {
   this->robot_device->write_byte_register(BIOLOID_ACTION_PAGE,page_id);
 }
 
-unsigned char CBioloid_Robot::action_get_current_page(void)
+unsigned char CBioloidRobot::action_get_current_page(void)
 {
   unsigned char page_id;
 
@@ -367,17 +387,17 @@ unsigned char CBioloid_Robot::action_get_current_page(void)
   return page_id;
 }
 
-void CBioloid_Robot::action_start(void)
+void CBioloidRobot::action_start(void)
 {
   this->robot_device->write_byte_register(BIOLOID_ACTION_CONTROL,0x01);
 }
 
-void CBioloid_Robot::action_stop(void)
+void CBioloidRobot::action_stop(void)
 {
   this->robot_device->write_byte_register(BIOLOID_ACTION_CONTROL,0x02);
 }
 
-bool CBioloid_Robot::action_is_page_running(void)
+bool CBioloidRobot::action_is_page_running(void)
 {
   unsigned char status;
 
@@ -389,7 +409,7 @@ bool CBioloid_Robot::action_is_page_running(void)
     return false;
 }*/
 
-CBioloid_Robot::~CBioloid_Robot()
+CBioloidRobot::~CBioloidRobot()
 {
   if(this->thread_server->get_thread_state(this->ext_int_thread_id)==active)
   {

src/bioloid_robot.h

@@ -17,7 +17,7 @@ typedef enum {USER1_LED,USER2_LED,RXD_LED,TXD_LED} led_t;
 /* available pushbuttons */
 typedef enum {USER_PB,RESET_PB,START_PB,MODE_PB} pushbutton_t;
 
-class CBioloid_Robot
+class CBioloidRobot
 {
   private:
     /* dynamixel interface */
@@ -45,8 +45,9 @@ class CBioloid_Robot
     CFunctor *mode_pb;
   protected:
     static void *ext_int_thread(void *param);
+    void clear_pending_interrupts(void);
   public:
-    CBioloid_Robot(const std::string &name,std::string &serial_dev,int baudrate, unsigned char id,gpio_ports port, int pin);
+    CBioloidRobot(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);
@@ -54,7 +55,7 @@ class CBioloid_Robot
     void blink_led(led_t led, int period_ms);
     bool is_button_pressed(pushbutton_t pushbutton);
     template<class T>
-    void assign_button_callback(pushbutton_t pushbutton,T *obj_ptr,void (T::*pb_callback)(CBioloid_Robot *))
+    void assign_button_callback(pushbutton_t pushbutton,T *obj_ptr,void (T::*pb_callback)(CBioloidRobot *))
     {
       switch(pushbutton)
       {
@@ -70,11 +71,41 @@ class CBioloid_Robot
                         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);
+         case RESET_PB: if(pb_callback!=NULL)
+                        {
+                          this->reset_pb=new CBioloidCallback<T>(obj_ptr,pb_callback);
+                          this->robot_device->write_byte_register(BIOLOID_RESET_PB_CNTRL,GPIO_INT_EN);//enable interrupts
+                        }
+                        else
+                        {
+                          delete this->reset_pb;
+                          this->reset_pb=NULL;
+                          this->robot_device->write_byte_register(BIOLOID_RESET_PB_CNTRL,0x00);//disable interrupts
+                        }
                         break;
-         case START_PB: this->robot_device->write_byte_register(BIOLOID_START_PB_CNTRL,GPIO_INT_EN);
+         case START_PB: if(pb_callback!=NULL)
+                        {
+                          this->start_pb=new CBioloidCallback<T>(obj_ptr,pb_callback);
+                          this->robot_device->write_byte_register(BIOLOID_START_PB_CNTRL,GPIO_INT_EN);//enable interrupts
+                        }
+                        else
+                        {
+                          delete this->start_pb;
+                          this->start_pb=NULL;
+                          this->robot_device->write_byte_register(BIOLOID_START_PB_CNTRL,0x00);//disable interrupts
+                        }
                         break;
-         case MODE_PB: this->robot_device->write_byte_register(BIOLOID_MODE_PB_CNTRL,GPIO_INT_EN);
+         case MODE_PB: if(pb_callback!=NULL)
+                       {
+                         this->mode_pb=new CBioloidCallback<T>(obj_ptr,pb_callback);
+                         this->robot_device->write_byte_register(BIOLOID_MODE_PB_CNTRL,GPIO_INT_EN);//enable interrupts
+                       }
+                       else
+                       {
+                         delete this->mode_pb;
+                         this->mode_pb=NULL;
+                         this->robot_device->write_byte_register(BIOLOID_MODE_PB_CNTRL,0x00);//disable interrupts
+                       }
                        break;
       }
     }
@@ -99,7 +130,7 @@ class CBioloid_Robot
     void action_start(void);
     void action_stop(void);
     bool action_is_page_running(void);*/
-    ~CBioloid_Robot();
+    ~CBioloidRobot();
 };
 
 #endif

src/bioloid_robot_callbacks.h

@@ -3,7 +3,7 @@
 
 #include "bioloid_robot_exceptions.h"
 
-class CBioloid_Robot;
+class CBioloidRobot;
 
 /**
  * \brief 
@@ -21,7 +21,7 @@ class CFunctor
      * \brief
      *
      */
-    virtual void call(CBioloid_Robot *robot){};
+    virtual void call(CBioloidRobot *robot){};
     /**
      * \brief 
      *
@@ -41,7 +41,7 @@ class CBioloidCallback : public CFunctor
     /**
      * \irief
      */
-    typedef void (T::*callback_fnct_ptr)(CBioloid_Robot *robot);                                         
+    typedef void (T::*callback_fnct_ptr)(CBioloidRobot *robot);                                         
     /**
      * \brief 
      *
@@ -76,7 +76,7 @@ class CBioloidCallback : public CFunctor
      * \brief 
      *
      */
-    virtual void call(CBioloid_Robot *robot)
+    virtual void call(CBioloidRobot *robot)
     {
       (this->obj_ptr->*this->callback_fnct)(robot);
     };

src/examples/bioloid_robot_test.cpp

@@ -9,10 +9,22 @@ class CCallbacks
 {
   public:
     CCallbacks(){};
-    void user_pb_callback(CBioloid_Robot *robot)
+    void user_pb_callback(CBioloidRobot *robot)
     {
       std::cout << "user push button pressed" << std::endl;
     }
+    void reset_pb_callback(CBioloidRobot *robot)
+    {
+      std::cout << "reset push button pressed" << std::endl;
+    }
+    void start_pb_callback(CBioloidRobot *robot)
+    {
+      std::cout << "start push button pressed" << std::endl;
+    }
+    void mode_pb_callback(CBioloidRobot *robot)
+    {
+      std::cout << "mode push button pressed" << std::endl;
+    }
     ~CCallbacks(){};
 };
 
@@ -22,8 +34,12 @@ int main(int argc, char *argv[])
   CCallbacks callbacks;
 
   try{
-    CBioloid_Robot tina("Tina",robot_device,921600,0x02,GPIO2,25);
-/*    for(i=0;i<4;i++)
+    CBioloidRobot tina("Tina",robot_device,921600,0x02,GPIO2,25);
+    tina.assign_button_callback(USER_PB,&callbacks,&CCallbacks::user_pb_callback);
+    tina.assign_button_callback(RESET_PB,&callbacks,&CCallbacks::reset_pb_callback);
+    tina.assign_button_callback(START_PB,&callbacks,&CCallbacks::start_pb_callback);
+    tina.assign_button_callback(MODE_PB,&callbacks,&CCallbacks::mode_pb_callback);
+    for(i=0;i<4;i++)
     {
       tina.toggle_led(RXD_LED);
       tina.toggle_led(TXD_LED);
@@ -45,8 +61,7 @@ int main(int argc, char *argv[])
     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);
+    tina.clear_led(USER2_LED);
     sleep(10);
 /*    for(i=0;i<100;i++)
     {