diff --git a/src/bioloid_robot.cpp b/src/bioloid_robot.cpp index 5f840838bed36597f36e23674e41ef9fefe6f7ae..32524af4b30fda695134daf538c0fe9974ce40cc 100644 --- a/src/bioloid_robot.cpp +++ b/src/bioloid_robot.cpp @@ -47,6 +47,9 @@ CBioloidRobot::CBioloidRobot(const std::string &name,std::string &serial_dev,int this->robot_device->read_byte_register(BIOLOID_MM_CNTRL,&this->manager_status); /* get the number of servos detected */ this->robot_device->read_byte_register(BIOLOID_MM_NUM_SERVOS,&this->manager_num_servos); + /* get the current action status */ + this->robot_device->read_byte_register(BIOLOID_ACTION_CNTRL,&this->action_status); + this->action_done=true; }catch(CException &e){ if(this->robot_device!=NULL) this->dyn_server->free_device(id); @@ -99,6 +102,9 @@ void *CBioloidRobot::ext_int_thread(void *param) robot->robot_device->read_byte_register(BIOLOID_MODE_PB_CNTRL,&value); if(value&GPIO_INT_FLAG) robot->mode_pb->call(robot); + robot->robot_device->read_byte_register(BIOLOID_ACTION_CNTRL,&value); + if(value&ACTION_INT_FLAG) + robot->action_done=true; } } } @@ -116,6 +122,7 @@ void CBioloidRobot::clear_pending_interrupts(void) 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); + this->robot_device->read_byte_register(BIOLOID_ACTION_CNTRL,&value); } else throw CBioloidRobotException(_HERE_,"Invalid robot device"); @@ -506,7 +513,6 @@ bool CBioloidRobot::mm_is_balance_enabled(void) } else throw CBioloidRobotException(_HERE_,"Invalid robot device"); - } void CBioloidRobot::mm_enable_power(void) @@ -736,7 +742,11 @@ unsigned char CBioloidRobot::action_get_current_page(void) void CBioloidRobot::action_start(void) { if(this->robot_device!=NULL) - this->robot_device->write_byte_register(BIOLOID_ACTION_CNTRL,ACTION_START); + { + this->action_status|=ACTION_START; + this->robot_device->write_byte_register(BIOLOID_ACTION_CNTRL,this->action_status); + this->action_done=false; + } else throw CBioloidRobotException(_HERE_,"Invalid robot device"); } @@ -744,19 +754,45 @@ void CBioloidRobot::action_start(void) void CBioloidRobot::action_stop(void) { if(this->robot_device!=NULL) - this->robot_device->write_byte_register(BIOLOID_ACTION_CNTRL,ACTION_STOP); + { + this->action_status|=ACTION_STOP; + this->robot_device->write_byte_register(BIOLOID_ACTION_CNTRL,this->action_status); + this->action_done=true; + } else throw CBioloidRobotException(_HERE_,"Invalid robot device"); } -bool CBioloidRobot::action_is_page_running(void) +void CBioloidRobot::action_enable_int(void) { - unsigned char status; + if(this->robot_device!=NULL) + { + this->action_status|=ACTION_INT_EN; + this->robot_device->write_byte_register(BIOLOID_ACTION_CNTRL,this->action_status); + } + else + throw CBioloidRobotException(_HERE_,"Invalid robot device"); +} +void CBioloidRobot::action_disable_int(void) +{ if(this->robot_device!=NULL) { - this->robot_device->read_byte_register(BIOLOID_ACTION_CNTRL,&status); - if(status&ACTION_STATUS) + this->action_status&=(~ACTION_INT_EN); + this->robot_device->write_byte_register(BIOLOID_ACTION_CNTRL,this->action_status); + } + else + throw CBioloidRobotException(_HERE_,"Invalid robot device"); +} + +bool CBioloidRobot::action_is_int_enabled(void) +{ + unsigned char value; + + if(this->robot_device!=NULL) + { + this->robot_device->read_byte_register(BIOLOID_ACTION_CNTRL,&value); + if(value&ACTION_INT_EN) return true; else return false; @@ -765,6 +801,32 @@ bool CBioloidRobot::action_is_page_running(void) throw CBioloidRobotException(_HERE_,"Invalid robot device"); } +bool CBioloidRobot::action_is_page_running(void) +{ + unsigned char status; + + if(this->robot_device!=NULL) + { + if(this->action_status&ACTION_INT_EN)// interrupts are enables + { + if(this->action_done) + return false; + else + return true; + } + else// polling + { + this->robot_device->read_byte_register(BIOLOID_ACTION_CNTRL,&status); + if(status&ACTION_STATUS) + return true; + else + return false; + } + } + else + throw CBioloidRobotException(_HERE_,"Invalid robot device"); +} + 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 d90094aecaf2b43ae30c2f8c95e521dd716a0e1c..8ae30f568bb8fefc1cd1dbf06ad2ddaee8c1fdb0 100644 --- a/src/bioloid_robot.h +++ b/src/bioloid_robot.h @@ -52,6 +52,9 @@ class CBioloidRobot /* motion manager variables */ unsigned char manager_status; unsigned char manager_num_servos; + /* action status */ + unsigned char action_status; + bool action_done; protected: static void *ext_int_thread(void *param); void clear_pending_interrupts(void); @@ -162,6 +165,9 @@ class CBioloidRobot unsigned char action_get_current_page(void); void action_start(void); void action_stop(void); + void action_enable_int(void); + void action_disable_int(void); + bool action_is_int_enabled(void); bool action_is_page_running(void); ~CBioloidRobot(); }; diff --git a/src/examples/bioloid_robot_test.cpp b/src/examples/bioloid_robot_test.cpp index a5e483cc123d4ea5be301cedd6b59779864065f8..22c5c9d6a29f4262459fbe662b65392f6c180fb9 100644 --- a/src/examples/bioloid_robot_test.cpp +++ b/src/examples/bioloid_robot_test.cpp @@ -115,7 +115,7 @@ int main(int argc, char *argv[]) tina.mm_disable_power(); tina.mm_stop();*/ std::cout << "Number of servos: " << (int)tina.mm_get_num_servos() << std::endl; - return 0; + tina.action_disable_int(); for(i=0;i<=MAX_NUM_SERVOS;i++) { tina.mm_enable_servo(i);