diff --git a/src/examples/darwin_manager_test.cpp b/src/examples/darwin_manager_test.cpp index 98b6fc8d3e69572b52170592e0f029b2e0a656fa..61bc10e6d42ff3bfee7fe6a2169558605fbadb18 100644 --- a/src/examples/darwin_manager_test.cpp +++ b/src/examples/darwin_manager_test.cpp @@ -9,8 +9,8 @@ std::string robot_device="/tmp/darwin_driver"; int main(int argc, char *argv[]) { -// int i=0,num_servos; -// unsigned int present_servos; + int i=0,num_servos; + unsigned int present_servos; // std::vector<int> servos; // std::vector<double> angles,speeds,accels,offsets; @@ -27,9 +27,28 @@ int main(int argc, char *argv[]) std::cout << "scanning ..." << std::endl; usleep(100000); } - std::cout << "num. devices: " << (int)darwin.manager_get_num_devices() << std::endl; -// num_servos=darwin.mm_get_num_servos(); -// present_servos=darwin.mm_get_present_servos(); + num_servos=darwin.manager_get_num_devices(); + std::cout << "num. devices: " << num_servos << std::endl; + present_servos=darwin.mm_get_present_servos(); + std::cout << "present servos: " << std::hex << present_servos << std::endl; + i=0; + while(i<num_servos) + { + if(present_servos&(0x00000001<<i)) + { + darwin.mm_enable_servo(i); + darwin.mm_assign_module(i,DARWIN_MM_ACTION); + } + i++; + } + darwin.action_load_page(9); + darwin.action_start(); + while(darwin.action_is_page_running()) + { + std::cout << "action running ... " << std::endl; + usleep(100000); + } + // std::cout << "Found " << num_servos << " servos " << std::endl; // std::cout << "Present servos: " << present_servos << std::hex << "0x" << present_servos << std::dec << std::endl; // std::cout << "Motion manager period " << darwin.mm_get_period() << " ms" << std::endl;