Implemented the head tracking functions.

Added an example to test the head tracking feature.

src/darwin_robot.cpp

@@ -85,6 +85,8 @@ CDarwinRobot::CDarwinRobot(const std::string &name,std::string &bus_id,int bus_s
       /* get the current joints status */
       for(i=0;i<JOINTS_NUM_GROUPS;i++)
         this->robot_device->read_byte_register(DARWIN_JOINT_GRP0_CNTRL+i*5,&this->joints_status[i]);
+      /* get the current head tracking status */
+      this->robot_device->read_byte_register(DARWIN_HEAD_CNTRL,&this->head_tracking_status);
     }
     else
     {
@@ -1581,67 +1583,229 @@ bool CDarwinRobot::joints_are_moving(joints_grp_t group)
 // head tracking interface
 void CDarwinRobot::head_set_pan_pid(double p,double i,double d,double i_clamp)
 {
- 
+  unsigned short int value;
+
+  if(this->robot_device!=NULL)
+  {
+    value=p*65536.0;
+    this->robot_device->write_word_register(DARWIN_HEAD_PAN_P_L,value);
+    usleep(100000);
+    value=i*65536.0;
+    this->robot_device->write_word_register(DARWIN_HEAD_PAN_I_L,value);
+    usleep(100000);
+    value=d*65536.0;
+    this->robot_device->write_word_register(DARWIN_HEAD_PAN_D_L,value);
+    usleep(100000);
+    value=i_clamp*128.0;
+    this->robot_device->write_word_register(DARWIN_HEAD_PAN_I_CLAMP_L,value);
+    usleep(100000);
+  }
+  else
+    throw CDarwinRobotException(_HERE_,"Invalid robot device");
 }
 
 void CDarwinRobot::head_get_pan_pid(double *p,double *i,double *d,double *i_clamp)
 {
- 
+  unsigned short int value;
+
+  if(this->robot_device!=NULL)
+  {
+    this->robot_device->read_word_register(DARWIN_HEAD_PAN_P_L,&value);
+    *p=((double)value)/65536.0;
+    this->robot_device->read_word_register(DARWIN_HEAD_PAN_I_L,&value);
+    *i=((double)value)/65536.0;
+    this->robot_device->read_word_register(DARWIN_HEAD_PAN_D_L,&value);
+    *d=((double)value)/65536.0;
+    this->robot_device->read_word_register(DARWIN_HEAD_PAN_I_CLAMP_L,&value);
+    *i_clamp=((double)value)/128.0;
+  }
+  else
+    throw CDarwinRobotException(_HERE_,"Invalid robot device");
 }
 
 void CDarwinRobot::head_set_tilt_pid(double p,double i,double d,double i_clamp)
 {
- 
+  unsigned short int value;
+
+  if(this->robot_device!=NULL)
+  {
+    value=p*65536.0;
+    this->robot_device->write_word_register(DARWIN_HEAD_TILT_P_L,value);
+    usleep(100000);
+    value=i*65536.0;
+    this->robot_device->write_word_register(DARWIN_HEAD_TILT_I_L,value);
+    usleep(100000);
+    value=d*65536.0;
+    this->robot_device->write_word_register(DARWIN_HEAD_TILT_D_L,value);
+    usleep(100000);
+    value=i_clamp*128.0;
+    this->robot_device->write_word_register(DARWIN_HEAD_TILT_I_CLAMP_L,value);
+    usleep(100000);
+  }
+  else
+    throw CDarwinRobotException(_HERE_,"Invalid robot device");
 }
 
 void CDarwinRobot::head_get_tilt_pid(double *p,double *i,double *d,double *i_clamp)
 {
- 
+  unsigned short int value;
+
+  if(this->robot_device!=NULL)
+  {
+    this->robot_device->read_word_register(DARWIN_HEAD_TILT_P_L,&value);
+    *p=((double)value)/65536.0;
+    this->robot_device->read_word_register(DARWIN_HEAD_TILT_I_L,&value);
+    *i=((double)value)/65536.0;
+    this->robot_device->read_word_register(DARWIN_HEAD_TILT_D_L,&value);
+    *d=((double)value)/65536.0;
+    this->robot_device->read_word_register(DARWIN_HEAD_TILT_I_CLAMP_L,&value);
+    *i_clamp=((double)value)/128.0;
+  }
+  else
+    throw CDarwinRobotException(_HERE_,"Invalid robot device");
 }
 
 void CDarwinRobot::head_set_pan_range(double max,double min)
 {
- 
+  unsigned short int value;
+
+  if(this->robot_device!=NULL)
+  {
+    value=max*128.0;   
+    this->robot_device->write_word_register(DARWIN_HEAD_MAX_PAN_L,value);
+    value=min*128.0;   
+    this->robot_device->write_word_register(DARWIN_HEAD_MIN_PAN_L,value);
+  }
+  else
+    throw CDarwinRobotException(_HERE_,"Invalid robot device");
 }
 
 void CDarwinRobot::head_get_pan_range(double *max,double *min)
 {
- 
+  unsigned short int value;
+
+  if(this->robot_device!=NULL)
+  {
+    this->robot_device->read_word_register(DARWIN_HEAD_MAX_PAN_L,&value);
+    *max=((double)value)/128.0; 
+    this->robot_device->read_word_register(DARWIN_HEAD_MIN_PAN_L,&value);
+    *min=((double)value)/128.0; 
+  }
+  else
+    throw CDarwinRobotException(_HERE_,"Invalid robot device");
 }
 
 void CDarwinRobot::head_set_tilt_range(double max,double min)
 {
- 
+  unsigned short int value;
+
+  if(this->robot_device!=NULL)
+  {
+    value=max*128.0;  
+    this->robot_device->write_word_register(DARWIN_HEAD_MAX_TILT_L,value);
+    value=min*128.0;
+    this->robot_device->write_word_register(DARWIN_HEAD_MIN_TILT_L,value);
+  }
+  else
+    throw CDarwinRobotException(_HERE_,"Invalid robot device");
 }
 
 void CDarwinRobot::head_get_tilt_range(double *max,double *min)
 {
- 
+  unsigned short int value;
+
+  if(this->robot_device!=NULL)
+  {
+    this->robot_device->read_word_register(DARWIN_HEAD_MAX_TILT_L,&value);
+    *max=((double)value)/128.0;
+    this->robot_device->read_word_register(DARWIN_HEAD_MIN_TILT_L,&value);
+    *min=((double)value)/128.0;
+  }
+  else
+    throw CDarwinRobotException(_HERE_,"Invalid robot device");
 }
 
 void CDarwinRobot::head_start_tracking(double pan,double tilt)
 {
- 
+  unsigned short int value;
+  unsigned char data[128];
+
+  if(this->robot_device!=NULL)
+  {
+    value=pan*128.0;
+    std::cout << "pan value:" << value << std::endl;
+    this->robot_device->write_word_register(DARWIN_HEAD_PAN_TARGET_L,value);
+    value=tilt*128.0;
+    std::cout << "tilt value:" << value << std::endl;
+    this->robot_device->write_word_register(DARWIN_HEAD_TILT_TARGET_L,value);
+    this->head_tracking_status|=HEAD_START;
+    this->robot_device->write_byte_register(DARWIN_HEAD_CNTRL,this->head_tracking_status);
+    this->robot_device->read_registers(DARWIN_HEAD_PAN_P_L,data,16);
+    for(unsigned int i=0;i<16;i++)
+      std::cout << "register: " << std::dec << (int)(DARWIN_HEAD_PAN_P_L + i) << std::hex << (int)data[i] << std::endl;
+    this->robot_device->read_registers(DARWIN_HEAD_CNTRL,data,13);
+    for(unsigned int i=0;i<13;i++)
+      std::cout << "register: " << std::dec << (int)(DARWIN_HEAD_CNTRL + i) << std::hex << (int)data[i] << std::endl;
+  }
+  else
+    throw CDarwinRobotException(_HERE_,"Invalid robot device");
 }
 
 void CDarwinRobot::head_stop_tracking(void)
 {
- 
+  if(this->robot_device!=NULL)
+  {
+    this->head_tracking_status|=HEAD_STOP;
+    this->robot_device->write_byte_register(DARWIN_HEAD_CNTRL,this->head_tracking_status);
+  }
+  else
+    throw CDarwinRobotException(_HERE_,"Invalid robot device");
 }
 
 bool CDarwinRobot::head_is_tracking(void)
-{
- 
+{ 
+  unsigned char value;
+
+  if(this->robot_device!=NULL)
+  {
+    this->robot_device->read_byte_register(DARWIN_HEAD_CNTRL,&value);
+    if(value&HEAD_STATUS)
+      return true;
+    else
+      return false;
+  }
+  else
+    throw CDarwinRobotException(_HERE_,"Invalid robot device");
 }
 
 void CDarwinRobot::head_set_new_target(double pan,double tilt)
 {
- 
+  unsigned short int value;
+
+  if(this->robot_device!=NULL)
+  {
+    value=pan*128.0;
+    this->robot_device->write_word_register(DARWIN_HEAD_PAN_TARGET_L,value);
+    value=tilt*128.0;
+    this->robot_device->write_word_register(DARWIN_HEAD_TILT_TARGET_L,value);
+  }
+  else
+    throw CDarwinRobotException(_HERE_,"Invalid robot device");
 }
 
-void CDarwinRobot::head_get_current_target(double pan,double tilt)
+void CDarwinRobot::head_get_current_target(double *pan,double *tilt)
 {
- 
+  unsigned short int value;
+
+  if(this->robot_device!=NULL)
+  {
+    this->robot_device->read_word_register(DARWIN_HEAD_PAN_TARGET_L,&value);
+    *pan=((double)value)/128.0;
+    this->robot_device->read_word_register(DARWIN_HEAD_TILT_TARGET_L,&value);
+    *tilt=((double)value)/128.0;
+  }
+  else
+    throw CDarwinRobotException(_HERE_,"Invalid robot device");
 }
 
 

src/darwin_robot.h

@@ -56,6 +56,8 @@ class CDarwinRobot
     unsigned char walk_status;
     /* joint group status */
     unsigned char joints_status[JOINTS_NUM_GROUPS];
+    /* head tracking status */
+    unsigned char head_tracking_status;
   public:
     CDarwinRobot(const std::string &name,std::string &bus_id,int bus_speed, unsigned char id);
     // GPIO interface
@@ -186,7 +188,7 @@ class CDarwinRobot
     void head_stop_tracking(void);
     bool head_is_tracking(void);
     void head_set_new_target(double pan,double tilt);
-    void head_get_current_target(double pan,double tilt);
+    void head_get_current_target(double *pan,double *tilt);
     ~CDarwinRobot();
 };
 

src/examples/CMakeLists.txt

@@ -33,6 +33,11 @@ ADD_EXECUTABLE(darwin_joint_motion_test darwin_joint_motion_test.cpp)
 # link necessary libraries
 TARGET_LINK_LIBRARIES(darwin_joint_motion_test darwin_robot)
 
+# create an example application
+ADD_EXECUTABLE(darwin_head_tracking_test darwin_head_tracking_test.cpp)
+# link necessary libraries
+TARGET_LINK_LIBRARIES(darwin_head_tracking_test darwin_robot)
+
 # create an example application
 #ADD_EXECUTABLE(darwin_kin darwin_kin.cpp)
 # link necessary libraries

src/examples/darwin_head_tracking_test.cpp

@@ -3,30 +3,35 @@
 
 #include <iostream>
 
-std::string robot_device="A603LOBS";
+//std::string robot_device="A603LOBS";
+std::string robot_device="A4008atn";
 
 int main(int argc, char *argv[])
 {
-  int num_v1_servos,num_v2_servos;
+  int num_servos;
+  unsigned int present_servos;
   std::vector<int> servos;
   std::vector<double> angles,speeds,accels;
 
   try{
-    CDarwinRobot darwin("Darwin",robot_device,1000000,0xC0);
+    CDarwinRobot darwin("Darwin",robot_device,1000000,0x02);
 
-    darwin.mm_get_num_servos(&num_v1_servos,&num_v2_servos);
-    std::cout << "Found " << num_v1_servos << " v1 servos and " << num_v2_servos << " v2 servos" << std::endl;
+    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_tracking_set_pan_pid(0.04,0.0,0.0);
-    darwin.head_tracking_set_tilt_pid(0.04,0.0,0.0);
+    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_tracking_set_target(0.0,90.0);
-    darwin.head_tracking_start();
+    darwin.head_start_tracking(45.0,90.0);
     sleep(10);
-    darwin.head_tracking_set_target(0.0,0.0);
+    darwin.head_set_new_target(0.0,0.0);
     sleep(10);
     darwin.mm_stop();
     darwin.mm_disable_power();