Added a function to get the servos present.

src/darwin_robot.cpp

@@ -457,6 +457,19 @@ unsigned char CDarwinRobot::mm_get_num_servos(void)
     throw CDarwinRobotException(_HERE_,"Invalid robot device");
 }
 
+unsigned int CDarwinRobot::mm_get_present_servos(void)
+{
+  unsigned int present_servos;
+
+  if(this->robot_device!=NULL)
+  {
+    this->robot_device->read_registers(DARWIN_MM_PRESENT_SERVOS1,(unsigned char *)&present_servos,4);
+    return present_servos;
+  }
+  else
+    throw CDarwinRobotException(_HERE_,"Invalid robot device");
+}
+
 void CDarwinRobot::mm_start(void)
 {
   if(this->robot_device!=NULL)

src/darwin_robot.h

@@ -83,6 +83,7 @@ class CDarwinRobot
     void mm_set_period(double period_ms);
     double mm_get_period(void);
     unsigned char mm_get_num_servos(void);
+    unsigned int mm_get_present_servos(void);
     void mm_start(void);
     void mm_stop(void);
     bool mm_is_running(void);

src/examples/darwin_action_test.cpp

@@ -10,15 +10,19 @@ std::string robot_device="A4008atn";
 int main(int argc, char *argv[])
 {
   int i=0,num_servos;
+  unsigned int present_servos;
 
   try{
     CDarwinRobot darwin("Darwin",robot_device,1000000,0x02);
 
     num_servos=darwin.mm_get_num_servos();
+    present_servos=darwin.mm_get_present_servos();
     std::cout << "Found " << num_servos << " servos " << std::endl;
+    std::cout << "Present servos: " << std::hex << "0x" << present_servos << std::dec << std::endl;
     // enable all servos and assign them to the action module
     darwin.mm_enable_power();
-    for(i=1;i<=num_servos;i++)
+    sleep(1);
+    for(i=0;i<MAX_NUM_SERVOS;i++)
     {
       darwin.mm_enable_servo(i);
       darwin.mm_assign_module(i,DARWIN_MM_ACTION);
@@ -28,7 +32,10 @@ int main(int argc, char *argv[])
     darwin.action_load_page(WALK_READY);
     darwin.action_start();
     while(darwin.action_is_page_running())
+    {
+      std::cout << "action running ... " << std::endl;
       usleep(100000);
+    }
     darwin.mm_stop();
     darwin.mm_disable_power();
   }catch(CException &e){