From fd40e84a863aabafb369c750f6252b0afabc7692 Mon Sep 17 00:00:00 2001 From: Sergi Hernandez Juan <shernand@iri.upc.edu> Date: Tue, 17 Mar 2020 22:57:08 +0100 Subject: [PATCH] Modified all the examples to iterate for all present servos. --- src/CMakeLists.txt | 5 +- src/examples/darwin_action_test.cpp | 43 +++++++---- src/examples/darwin_head_tracking_test.cpp | 6 +- src/examples/darwin_joint_motion_test.cpp | 6 +- src/examples/darwin_manager_test.cpp | 89 +--------------------- src/examples/darwin_walking_test.cpp | 11 ++- 6 files changed, 49 insertions(+), 111 deletions(-) diff --git a/src/CMakeLists.txt b/src/CMakeLists.txt index 8c6a068..2682c6b 100644 --- a/src/CMakeLists.txt +++ b/src/CMakeLists.txt @@ -13,7 +13,7 @@ INCLUDE (${PROJECT_SOURCE_DIR}/FindKDL.cmake) # driver source files SET(robot_sources darwin_robot_base.cpp darwin_robot_exceptions.cpp darwin_imu.cpp darwin_dyn_manager.cpp darwin_mmanager.cpp darwin_action.cpp darwin_balance.cpp darwin_joint_motion.cpp darwin_walk.cpp darwin_head_tracking.cpp) # application header files -SET(robot_headers ../include/darwin_robot.h ../include/darwin_robot_exceptions.h ../include/darwin_imu.h ../include/darwin_dyn_manager.h ../include/darwin_mmanager.h ../include/darwin_action.h ../include/darwin_balance.h ../include/darwin_joint_motion.h ../include/darwin_walk.h ../include/darwin_head_tracking.h) +SET(robot_headers ../include/darwin_robot_base.h ../include/darwin_robot_exceptions.h ../include/darwin_imu.h ../include/darwin_dyn_manager.h ../include/darwin_mmanager.h ../include/darwin_action.h ../include/darwin_balance.h ../include/darwin_joint_motion.h ../include/darwin_walk.h ../include/darwin_head_tracking.h) # locate the necessary dependencies FIND_PACKAGE(iriutils REQUIRED) @@ -71,8 +71,7 @@ INSTALL(TARGETS darwin_robot LIBRARY DESTINATION lib/iridrivers ARCHIVE DESTINATION lib/iridrivers) INSTALL(FILES ${robot_headers} DESTINATION include/iridrivers) -INSTALL(FILES ${DARWIN_FW_PATH}/include/darwin_registers.h DESTINATION include/iridrivers) -INSTALL(FILES ${DARWIN_FW_PATH}/include/action_id.h DESTINATION include/iridrivers) +INSTALL(FILES ../include/darwin_registers.h DESTINATION include/iridrivers) IF(KDL_FOUND) INSTALL(FILES ${kin_arm_headers} DESTINATION include/iridrivers) INSTALL(TARGETS darwin_arm_kinematics diff --git a/src/examples/darwin_action_test.cpp b/src/examples/darwin_action_test.cpp index 016337f..777b3b9 100644 --- a/src/examples/darwin_action_test.cpp +++ b/src/examples/darwin_action_test.cpp @@ -8,7 +8,7 @@ std::string robot_device="/tmp/darwin_driver"; int main(int argc, char *argv[]) { - int i=0,num_servos; + int i=0,num_servos,remaining_servos; unsigned int present_servos; try{ @@ -31,30 +31,43 @@ int main(int argc, char *argv[]) present_servos=darwin.get_present_servos(); std::cout << "present servos: " << std::hex << present_servos << std::endl; i=0; - while(i<num_servos) + remaining_servos=num_servos; + while(remaining_servos>0) { if(present_servos&(0x00000001<<i)) { darwin.enable_servo(i); darwin.assign_module(i,DARWIN_MM_ACTION); + remaining_servos--; } i++; } darwin.start(); - action.load_page(12); - action.start(); - while(action.is_page_running()) + for(unsigned int i=0;i<20;i++) { - std::cout << "action running ... " << std::endl; - usleep(100000); - } - sleep(2); - action.load_page(13); - action.start(); - while(action.is_page_running()) - { - std::cout << "action running ... " << std::endl; - usleep(100000); + action.load_page(7); + action.start(); + while(action.is_page_running()) + { + std::cout << "action running ... " << std::endl; + usleep(100000); + } + sleep(2); + action.load_page(12); + action.start(); + while(action.is_page_running()) + { + std::cout << "action running ... " << std::endl; + usleep(100000); + } + sleep(2); + action.load_page(13); + action.start(); + while(action.is_page_running()) + { + std::cout << "action running ... " << std::endl; + usleep(100000); + } } darwin.stop(); }catch(CException &e){ diff --git a/src/examples/darwin_head_tracking_test.cpp b/src/examples/darwin_head_tracking_test.cpp index c1719f3..6dcb357 100644 --- a/src/examples/darwin_head_tracking_test.cpp +++ b/src/examples/darwin_head_tracking_test.cpp @@ -8,7 +8,7 @@ std::string robot_device="/tmp/darwin_driver"; int main(int argc, char *argv[]) { - int i=0,num_servos; + int i=0,num_servos,remaining_servos; unsigned int present_servos; try{ @@ -31,12 +31,14 @@ int main(int argc, char *argv[]) present_servos=darwin.get_present_servos(); std::cout << "present servos: " << std::hex << present_servos << std::endl; i=0; - while(i<MAX_NUM_SERVOS) + remaining_servos=num_servos; + while(remaining_servos>0) { if(present_servos&(0x00000001<<i)) { darwin.enable_servo(i); darwin.assign_module(i,DARWIN_MM_HEAD); + remaining_servos--; } i++; } diff --git a/src/examples/darwin_joint_motion_test.cpp b/src/examples/darwin_joint_motion_test.cpp index 2c63842..30473d7 100644 --- a/src/examples/darwin_joint_motion_test.cpp +++ b/src/examples/darwin_joint_motion_test.cpp @@ -8,7 +8,7 @@ std::string robot_device="/tmp/darwin_driver"; int main(int argc, char *argv[]) { - int i=0,num_servos; + int i=0,num_servos,remaining_servos; unsigned int present_servos; std::vector<unsigned char> servos; std::vector<double> angles,speeds,accels; @@ -33,12 +33,14 @@ int main(int argc, char *argv[]) present_servos=darwin.get_present_servos(); std::cout << "present servos: " << std::hex << present_servos << std::endl; i=0; - while(i<num_servos) + remaining_servos=num_servos; + while(remaining_servos>0) { if(present_servos&(0x00000001<<i)) { darwin.enable_servo(i); darwin.assign_module(i,DARWIN_MM_JOINTS); + remaining_servos--; } i++; } diff --git a/src/examples/darwin_manager_test.cpp b/src/examples/darwin_manager_test.cpp index f695e1b..bbe8889 100644 --- a/src/examples/darwin_manager_test.cpp +++ b/src/examples/darwin_manager_test.cpp @@ -7,7 +7,7 @@ std::string robot_device="/tmp/darwin_driver"; int main(int argc, char *argv[]) { - int i=0,j=0,num_servos; + int i=0,j=0,num_servos,remaining_servos; unsigned int present_servos; std::vector<double> offsets,angles; @@ -31,12 +31,14 @@ int main(int argc, char *argv[]) present_servos=darwin.get_present_servos(); std::cout << "present servos: " << std::hex << present_servos << std::endl; i=0; - while(i<num_servos) + remaining_servos=num_servos; + while(remaining_servos>0) { if(present_servos&(0x00000001<<i)) { darwin.enable_servo(i); darwin.assign_module(i,DARWIN_MM_ACTION); + remaining_servos--; } i++; } @@ -60,89 +62,6 @@ int main(int argc, char *argv[]) usleep(100000); } darwin.stop(); - - //darwin.start(); -// darwin.action_load_page(12); -// darwin.action_start(); -// while(darwin.action_is_page_running()) -// { -// std::cout << "action running ... " << std::endl; -// usleep(100000); -// } -// sleep(2); -// darwin.action_load_page(13); -// 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; - // execute the walk ready action -// std::cout << "Assign servos to the action module" << std::endl; -/* - for(i=1;i<=20;i++) - { - darwin.mm_enable_servo(i); - darwin.mm_assign_module(i,DARWIN_MM_ACTION); - } - for(i=0;i<=MAX_NUM_SERVOS;i++) - { - if(darwin.mm_is_servo_enabled(i)) - std::cout << "Servo " << i << " enabled" << std::endl; - else - std::cout << "Servo " << i << " disabled" << std::endl; - std::cout << "Servo " << i; - switch(darwin.mm_get_module(i)) - { - case DARWIN_MM_NONE: std::cout << " not assigned to any module" << std::endl; - break; - case DARWIN_MM_ACTION: std::cout << " assigned to the action module" << std::endl; - break; - case DARWIN_MM_WALKING: std::cout << " assigned to the walking module" << std::endl; - break; - case DARWIN_MM_JOINTS: std::cout << " assigned to the joints module" << std::endl; - break; - case DARWIN_MM_HEAD: std::cout << " assigned to the head module" << std::endl; - break; - default: std::cout << " assigned to an unknown module" << std::endl; - break; - } - } - if(darwin.mm_is_running()) - std::cout << "Motion manager is running" << std::endl; - else - std::cout << "Motion manager is not running" << std::endl; - darwin.mm_start(); - if(darwin.mm_is_running()) - std::cout << "Motion manager is running" << std::endl; - else - std::cout << "Motion manager is not running" << std::endl; - sleep(10); - darwin.mm_stop(); - if(darwin.mm_is_running()) - std::cout << "Motion manager is running" << std::endl; - else - std::cout << "Motion manager is not running" << std::endl; - if(darwin.mm_is_power_enabled()) - std::cout << "Servos are powered" << std::endl; - else - std::cout << "Servos are not powered" << std::endl; - darwin.mm_enable_power(); - if(darwin.mm_is_power_enabled()) - std::cout << "Servos are powered" << std::endl; - else - std::cout << "Servos are not powered" << std::endl; - darwin.mm_disable_power(); - if(darwin.mm_is_power_enabled()) - std::cout << "Servos are powered" << std::endl; - else - std::cout << "Servos are not powered" << std::endl; - std::cout << "Servo offsets: " << std::endl; -*/ }catch(CException &e){ std::cout << e.what() << std::endl; } diff --git a/src/examples/darwin_walking_test.cpp b/src/examples/darwin_walking_test.cpp index d74f3d2..2d709b4 100644 --- a/src/examples/darwin_walking_test.cpp +++ b/src/examples/darwin_walking_test.cpp @@ -12,7 +12,7 @@ std::string robot_device="/tmp/darwin_driver"; int main(int argc, char *argv[]) { - int i=0,num_servos; + int i=0,num_servos,remaining_servos; unsigned int present_servos; try{ @@ -38,12 +38,14 @@ int main(int argc, char *argv[]) present_servos=darwin.get_present_servos(); std::cout << "present servos: " << std::hex << present_servos << std::endl; i=0; - while(i<num_servos) + remaining_servos=num_servos; + while(remaining_servos>0) { if(present_servos&(0x00000001<<i)) { darwin.enable_servo(i); darwin.assign_module(i,DARWIN_MM_ACTION); + remaining_servos--; } i++; } @@ -67,12 +69,13 @@ int main(int argc, char *argv[]) std::cout << "calibration done" << std::endl; balance.enable(); i=0; - while(i<num_servos) + remaining_servos=num_servos; + while(remaining_servos>0) { if(present_servos&(0x00000001<<i)) { - darwin.enable_servo(i); darwin.assign_module(i,DARWIN_MM_WALKING); + remaining_servos--; } i++; } -- GitLab