diff --git a/src/CMakeLists.txt b/src/CMakeLists.txt index 8c6a06879f30ef8a4e47d15e165065736a09d4ff..2682c6bb18ef3850fae0f56f289bbc10f22da30d 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 016337f778826f9276c18b7df4a929690e0b3fde..777b3b9e8d0ec721aecbe3ba850149f84e1252a1 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 c1719f33bc745ed16ac1da1f08c5f8f1785aad23..6dcb357d18676f6dbf40fc91b0be846d5f1c1eef 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 2c63842e5812f1b4893ae540a561bc73996d3359..30473d712a6c8a96d80e10aa109e04bf9934140f 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 f695e1bf3348786b6d3c560a2d4b6a89813f5fd7..bbe888959949200dbbc65d370a98f560cca4f9f3 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 d74f3d23f8b40d0b01f7c0f86652e46ba4ff0cee..2d709b47efb29bec71c2ad11fdf3f67943d228ea 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++; }