Updated the head tracking example application.

src/examples/CMakeLists.txt

@@ -39,21 +39,21 @@ ADD_EXECUTABLE(darwin_joint_motion_test darwin_joint_motion_test.cpp)
 TARGET_LINK_LIBRARIES(darwin_joint_motion_test darwin_robot)
 
 # create an example application
-#ADD_EXECUTABLE(darwin_head_tracking_test darwin_head_tracking_test.cpp)
+ADD_EXECUTABLE(darwin_head_tracking_test darwin_head_tracking_test.cpp)
 # link necessary libraries
-#TARGET_LINK_LIBRARIES(darwin_head_tracking_test darwin_robot)
+TARGET_LINK_LIBRARIES(darwin_head_tracking_test darwin_robot)
 
-#IF(KDL_FOUND)
+IF(KDL_FOUND)
   # create an example application
-#  ADD_EXECUTABLE(darwin_arm_kin darwin_arm_kin.cpp)
+  ADD_EXECUTABLE(darwin_arm_kin darwin_arm_kin.cpp)
   # link necessary libraries
-#  TARGET_LINK_LIBRARIES(darwin_arm_kin darwin_arm_kinematics)
+  TARGET_LINK_LIBRARIES(darwin_arm_kin darwin_arm_kinematics)
 
   # create an example application
-#  ADD_EXECUTABLE(darwin_leg_kin darwin_leg_kin.cpp)
+  ADD_EXECUTABLE(darwin_leg_kin darwin_leg_kin.cpp)
   # link necessary libraries
-#  TARGET_LINK_LIBRARIES(darwin_leg_kin darwin_leg_kinematics)
-#ENDIF(KDL_FOUND)
+  TARGET_LINK_LIBRARIES(darwin_leg_kin darwin_leg_kinematics)
+ENDIF(KDL_FOUND)
 
 # create an example application
 #ADD_EXECUTABLE(darwin_smart_charger_test darwin_smart_charger_test.cpp)

src/examples/darwin_head_tracking_test.cpp

-#include "darwin_robot.h"
+#include "darwin_mmanager.h"
+#include "darwin_head_tracking.h"
 #include "darwin_robot_exceptions.h"
 
 #include <iostream>
 
-std::string robot_device="/dev/pts/20";
-//std::string robot_device="A603LOBS";
+std::string robot_device="/tmp/darwin_driver";
 
 int main(int argc, char *argv[])
 {
-  int num_servos;
+  int i=0,num_servos;
   unsigned int present_servos;
-  std::vector<int> servos;
-  std::vector<double> angles,speeds,accels;
 
   try{
-    CDarwinRobot darwin("Darwin",robot_device,1000000,0x02,true);
-
-    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;
-    darwin.mm_enable_power();
-    sleep(1);
-    darwin.head_set_pan_pid(0.04,0.0,0.0,0.0);
-    darwin.head_set_tilt_pid(0.04,0.0,0.0,0.0);
-    darwin.mm_enable_servo(20);
-    darwin.mm_assign_module(20,DARWIN_MM_HEAD);
-    darwin.mm_enable_servo(19);
-    darwin.mm_assign_module(19,DARWIN_MM_HEAD);
-    darwin.mm_start();
-    darwin.head_start_tracking(45.0,90.0);
+    CDarwinMManager darwin("Darwin",robot_device,1000000,0x01,true);
+    CDarwinHeadTracking head("Darwin",robot_device,1000000,0x01,true);
+    std::cout << "Manager period: " << darwin.get_base_period() << std::endl;
+    std::cout << "Number of modules: " << darwin.get_num_modules() << std::endl;
+    std::cout << "Number of masters: " << darwin.get_num_masters() << std::endl;
+    std::cout << "Motion manager period: " << darwin.get_period() << std::endl;
+    darwin.start_scan();
+    while(darwin.is_scanning())
+    {
+      std::cout << "scanning ..." << std::endl;
+      usleep(100000);
+    }
+    num_servos=darwin.get_num_devices();
+    std::cout << "num. devices: " << num_servos << std::endl;
+    present_servos=darwin.get_present_devices();
+    std::cout << "present devices: " << std::hex << present_servos << std::endl;
+    present_servos=darwin.get_present_servos();
+    std::cout << "present servos: " << std::hex << present_servos << std::endl;
+    i=0;
+    while(i<MAX_NUM_SERVOS)
+    {
+      if(present_servos&(0x00000001<<i))
+      {
+        darwin.enable_servo(i);
+        darwin.assign_module(i,DARWIN_MM_HEAD);
+      }
+      i++;
+    }
+    head.set_pan_pid(0.04,0.0,0.0,0.0);
+    head.set_tilt_pid(0.04,0.0,0.0,0.0);
+    darwin.start();
+    head.start_tracking(45.0,90.0);
     sleep(3);
-    darwin.head_set_new_target(0.0,0.0);
+    head.set_new_target(0.0,0.0);
     sleep(3);
-    darwin.head_stop_tracking();
+    head.stop_tracking();
     sleep(1);
-    darwin.head_start_tracking(45.0,90.0);
+    head.start_tracking(45.0,90.0);
     sleep(3);
-    darwin.head_set_new_target(0.0,0.0);
+    head.set_new_target(0.0,0.0);
     sleep(3);
-    darwin.mm_stop();
-    darwin.mm_disable_power();
+    darwin.stop();
   }catch(CException &e){
     std::cout << e.what() << std::endl;
   }