Modified all the examples to iterate for all present servos.

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

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){

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++;
     }

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++;
     }

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;
   }

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++;
     }