diff --git a/src/bioloid_robot.cpp b/src/bioloid_robot.cpp
index 71dd216876d1a4341dd7209748b20842b12f2a64..91b493d2cf0a8ccc047ea3e85e151a0652c8051b 100644
--- a/src/bioloid_robot.cpp
+++ b/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)
{
diff --git a/src/bioloid_robot.h b/src/bioloid_robot.h
index a81a343b79dfb337278b8dfc8605a477bd0c6003..72b86943749a70286517520f445cf3395301b25c 100644
--- a/src/bioloid_robot.h
+++ b/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
diff --git a/src/bioloid_robot_callbacks.h b/src/bioloid_robot_callbacks.h
index 73f293ba487cd583f9b1417921b3a68d940bf2d2..a22788747167b57e9e46240d74fc29724f027ae5 100755
--- a/src/bioloid_robot_callbacks.h
+++ b/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);
};
diff --git a/src/examples/bioloid_robot_test.cpp b/src/examples/bioloid_robot_test.cpp
index 9c5e09769bae8f39bede7d52464351f0b335af13..63a350fae7f93db4f119094451ced28b49d2be33 100644
--- a/src/examples/bioloid_robot_test.cpp
+++ b/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++)
{