diff --git a/src/bioloid_robot.cpp b/src/bioloid_robot.cpp index ae2a15fc3b869b69f3f94301686d025a1d0c6de7..8fd975153743102d53f6c32b7b15f007843445df 100644 --- a/src/bioloid_robot.cpp +++ b/src/bioloid_robot.cpp @@ -41,6 +41,8 @@ CBioloidRobot::CBioloidRobot(const std::string &name,std::string &serial_dev,int this->start_pb=NULL; this->mode_pb=NULL; this->clear_pending_interrupts(); + /* get the current zigbee status */ + this->robot_device->read_byte_register(BIOLOID_ZIGBEE_CNTRL,&this->zigbee_status); }catch(CException &e){ if(this->robot_device!=NULL) this->dyn_server->free_device(id); @@ -308,6 +310,83 @@ void CBioloidRobot::adc_disable(void) throw CBioloidRobotException(_HERE_,"Invalid robot device"); } +// zigbee interface +void CBioloidRobot::zigbee_enable_power(void) +{ + if(this->robot_device!=NULL) + { + this->zigbee_status|=0x01; + this->robot_device->write_byte_register(BIOLOID_ZIGBEE_CNTRL,this->zigbee_status); + } + else + throw CBioloidRobotException(_HERE_,"Invalid robot device"); +} + +void CBioloidRobot::zigbee_disable_power(void) +{ + if(this->robot_device!=NULL) + { + this->zigbee_status&=0xFE; + this->robot_device->write_byte_register(BIOLOID_ZIGBEE_CNTRL,this->zigbee_status); + } + else + throw CBioloidRobotException(_HERE_,"Invalid robot device"); +} + +bool CBioloidRobot::is_zigbee_power_on(void) +{ + unsigned char value; + + if(this->robot_device!=NULL) + { + this->robot_device->read_byte_register(BIOLOID_ZIGBEE_CNTRL,&value); + if(value&0x01) + return true; + else + return false; + } + else + throw CBioloidRobotException(_HERE_,"Invalid robot device"); +} + +void CBioloidRobot::zigbee_enable(void) +{ + if(this->robot_device!=NULL) + { + this->zigbee_status|=0x02; + this->robot_device->write_byte_register(BIOLOID_ZIGBEE_CNTRL,this->zigbee_status); + } + else + throw CBioloidRobotException(_HERE_,"Invalid robot device"); +} + +void CBioloidRobot::zigbee_disable(void) +{ + if(this->robot_device!=NULL) + { + this->zigbee_status&=0xFD; + this->robot_device->write_byte_register(BIOLOID_ZIGBEE_CNTRL,this->zigbee_status); + } + else + throw CBioloidRobotException(_HERE_,"Invalid robot device"); +} + +bool CBioloidRobot::is_zigbee_enabled(void) +{ + unsigned char value; + + if(this->robot_device!=NULL) + { + this->robot_device->read_byte_register(BIOLOID_ZIGBEE_CNTRL,&value); + if(value&0x02) + return true; + else + return false; + } + else + throw CBioloidRobotException(_HERE_,"Invalid robot device"); +} + // motion manager interface /*void CBioloidRobot::mm_set_period(double period_ms) { diff --git a/src/bioloid_robot.h b/src/bioloid_robot.h index 381c37bae3c913da8dc2f60efb10bd58994b3f53..5bb395459031e4cba69eafee04555883e3cdb8da 100644 --- a/src/bioloid_robot.h +++ b/src/bioloid_robot.h @@ -45,6 +45,8 @@ class CBioloidRobot CFunctor *reset_pb; CFunctor *start_pb; CFunctor *mode_pb; + /* zigbee status */ + unsigned char zigbee_status; protected: static void *ext_int_thread(void *param); void clear_pending_interrupts(void); @@ -123,6 +125,13 @@ class CBioloidRobot double adc_get_temperature(void); double adc_get_vrefint(void); void adc_disable(void); + // zigbee interface + void zigbee_enable_power(void); + void zigbee_disable_power(void); + bool is_zigbee_power_on(void); + void zigbee_enable(void); + void zigbee_disable(void); + bool is_zigbee_enabled(void); // motion manager interface /* void mm_set_period(double period_ms); unsigned char mm_get_num_servos(void); diff --git a/src/examples/bioloid_robot_test.cpp b/src/examples/bioloid_robot_test.cpp index b1801d727dc10b18ef9b5cc89e4fa549e39485d6..7f4e4a6584e69dedd4ecee7be837f8f474b4e262 100644 --- a/src/examples/bioloid_robot_test.cpp +++ b/src/examples/bioloid_robot_test.cpp @@ -63,7 +63,7 @@ int main(int argc, char *argv[]) tina.clear_led(USER1_LED); tina.clear_led(USER2_LED); sleep(10);*/ - for(i=0;i<100;i++) +/* for(i=0;i<100;i++) { std::cout << "temperature: " << tina.adc_get_temperature() << std::endl; std::cout << "Vref_int: " << tina.adc_get_vrefint() << std::endl; @@ -74,7 +74,10 @@ int main(int argc, char *argv[]) std::cout << "Channel 5: " << tina.adc_get_value(ADC_CH5) << std::endl; std::cout << "Channel 6: " << tina.adc_get_value(ADC_CH6) << std::endl; usleep(100000); - } + }*/ + tina.zigbee_enable_power(); + sleep(10); + tina.zigbee_disable_power(); }catch(CException &e){ std::cout << e.what() << std::endl; }