diff --git a/include/darwin_dyn_manager.h b/include/darwin_dyn_manager.h index f66ceedd850ee342b913cfc83071c6cddf1f024f..f8a933629968f830ef19c0f52b9d6445e0672149 100644 --- a/include/darwin_dyn_manager.h +++ b/include/darwin_dyn_manager.h @@ -20,6 +20,9 @@ class CDarwinDynManager : public CDarwinRobotBase bool is_running(void); void start_scan(void); bool is_scanning(void); + void enable_power(void); + void disable_power(void); + bool is_power_enabled(void); unsigned char get_num_devices(void); unsigned int get_present_devices(void); ~CDarwinDynManager(); diff --git a/include/darwin_mmanager.h b/include/darwin_mmanager.h index 116672479879315358f5a1f4ea95423ad28bb4c7..e5080a037641bbc75701adf837770285adc71ac7 100644 --- a/include/darwin_mmanager.h +++ b/include/darwin_mmanager.h @@ -25,9 +25,6 @@ class CDarwinMManager : public CDarwinDynManager void assign_module(unsigned char servo_id, mm_mode_t mode); void assign_module(std::string &servo,std::string &module); mm_mode_t get_module(unsigned char servo_id); -// void enable_power(void); -// void disable_power(void); -// bool is_power_enabled(void); std::vector<double> get_servo_angles(void); double get_servo_angle(unsigned char servo_id); void set_servo_offset(unsigned char servo_id,double offset); diff --git a/src/darwin_dyn_manager.cpp b/src/darwin_dyn_manager.cpp index f81fd938dc8e3afd6b573c40f12d91cbaf63a769..cc5e6cd489305b90f0ad391b8ac9c4a3e709bbe9 100644 --- a/src/darwin_dyn_manager.cpp +++ b/src/darwin_dyn_manager.cpp @@ -79,6 +79,27 @@ bool CDarwinDynManager::is_scanning(void) return scanning&MANAGER_SCANNING; } +void CDarwinDynManager::enable_power(void) +{ + this->is_valid(); + this->robot_device->write_byte_register(MANAGER_CONTROL,MANAGER_ENABLE_POWER); +} + +void CDarwinDynManager::disable_power(void) +{ + this->is_valid(); + this->robot_device->write_byte_register(MANAGER_CONTROL,MANAGER_DISABLE_POWER); +} + +bool CDarwinDynManager::is_power_enabled(void) +{ + unsigned char powered; + + this->is_valid(); + this->robot_device->read_byte_register(MANAGER_CONTROL,&powered); + return powered&MANAGER_POWERED; +} + unsigned char CDarwinDynManager::get_num_devices(void) { unsigned char num; diff --git a/src/examples/darwin_manager_test.cpp b/src/examples/darwin_manager_test.cpp index 26a5fd10559d2042c775c722a1957ae5d587c2f5..fe09ca6507cd473bef49f7e9a37447685b9aad31 100644 --- a/src/examples/darwin_manager_test.cpp +++ b/src/examples/darwin_manager_test.cpp @@ -21,6 +21,9 @@ int main(int argc, char *argv[]) std::cout << "Number of modules: " << darwin.get_num_modules() << std::endl; std::cout << "Number of masters: " << darwin.get_num_masters() << std::endl; std::cout << "Motion manager period: " << darwin.get_period() << std::endl; + + darwin.enable_power(); + sleep(2); darwin.stop(); darwin.start_scan(); while(darwin.is_scanning()) @@ -66,6 +69,7 @@ int main(int argc, char *argv[]) usleep(100000); } darwin.stop(); + darwin.disable_power(); }catch(CException &e){ std::cout << e.what() << std::endl; }