Added a new class to interface with the head tracking motion module.

include/darwin_head_tracking.h

+#ifndef _DARWIN_HEAD_TRACKING_H
+#define _DARWIN_HEAD_TRACKING_H
+
+#include "darwin_robot_base.h"
+#include "darwin_registers.h"
+
+class CDarwinHeadTracking : public CDarwinRobotBase
+{
+  public:
+    CDarwinHeadTracking(const std::string &name,std::string &bus_id,int bus_speed, unsigned char id,bool sim=false);
+    void set_pan_pid(double p,double i,double d,double i_clamp);
+    void get_pan_pid(double *p,double *i,double *d,double *i_clamp);
+    void set_tilt_pid(double p,double i,double d,double i_clamp);
+    void get_tilt_pid(double *p,double *i,double *d,double *i_clamp);
+    void set_pan_range(double max,double min);
+    void get_pan_range(double *max,double *min);
+    void set_tilt_range(double max,double min);
+    void get_tilt_range(double *max,double *min);
+    void start_tracking(double pan,double tilt);
+    void stop_tracking(void);
+    bool is_tracking(void);
+    void set_new_target(double pan,double tilt);
+    void get_current_target(double *pan,double *tilt);
+    ~CDarwinHeadTracking();
+};
+
+#endif

src/CMakeLists.txt

@@ -11,9 +11,9 @@ SET(KDL_INSTALL /opt/ros/indigo)
 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)
+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)
+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)
 
 # locate the necessary dependencies
 FIND_PACKAGE(iriutils REQUIRED)

src/darwin_head_tracking.cpp

+#include "darwin_head_tracking.h"
+#include "darwin_robot_exceptions.h"
+
+CDarwinHeadTracking::CDarwinHeadTracking(const std::string &name,std::string &bus_id,int bus_speed, unsigned char id,bool sim) : CDarwinRobotBase(name,bus_id,bus_speed,id,sim)
+{
+}
+
+// head tracking interface
+void CDarwinHeadTracking::set_pan_pid(double p,double i,double d,double i_clamp)
+{
+  unsigned short int value;
+
+  this->is_valid();  
+  value=p*65536.0;
+  this->robot_device->write_word_register(HEAD_TRACKING_PAN_P_GAIN,value);
+  usleep(10000);
+  value=i*65536.0;
+  this->robot_device->write_word_register(HEAD_TRACKING_PAN_I_GAIN,value);
+  usleep(10000);
+  value=d*65536.0;
+  this->robot_device->write_word_register(HEAD_TRACKING_PAN_D_GAIN,value);
+  usleep(10000);
+  value=i_clamp*128.0;
+  this->robot_device->write_word_register(HEAD_TRACKING_PAN_I_CLAMP,value);
+  usleep(10000);
+}
+
+void CDarwinHeadTracking::get_pan_pid(double *p,double *i,double *d,double *i_clamp)
+{
+  unsigned short int value;
+
+  this->is_valid();
+  this->robot_device->read_word_register(HEAD_TRACKING_PAN_P_GAIN,&value);
+  *p=((double)value)/65536.0;
+  this->robot_device->read_word_register(HEAD_TRACKING_PAN_I_GAIN,&value);
+  *i=((double)value)/65536.0;
+  this->robot_device->read_word_register(HEAD_TRACKING_PAN_D_GAIN,&value);
+  *d=((double)value)/65536.0;
+  this->robot_device->read_word_register(HEAD_TRACKING_PAN_I_CLAMP,&value);
+  *i_clamp=((double)value)/128.0;
+}
+
+void CDarwinHeadTracking::set_tilt_pid(double p,double i,double d,double i_clamp)
+{
+  unsigned short int value;
+
+  this->is_valid();
+  value=p*65536.0;
+  this->robot_device->write_word_register(HEAD_TRACKING_TILT_P_GAIN,value);
+  usleep(10000);
+  value=i*65536.0;
+  this->robot_device->write_word_register(HEAD_TRACKING_TILT_I_GAIN,value);
+  usleep(10000);
+  value=d*65536.0;
+  this->robot_device->write_word_register(HEAD_TRACKING_TILT_D_GAIN,value);
+  usleep(10000);
+  value=i_clamp*128.0;
+  this->robot_device->write_word_register(HEAD_TRACKING_TILT_I_CLAMP,value);
+  usleep(10000);
+}
+
+void CDarwinHeadTracking::get_tilt_pid(double *p,double *i,double *d,double *i_clamp)
+{
+  unsigned short int value;
+
+  this->is_valid();
+  this->robot_device->read_word_register(HEAD_TRACKING_TILT_P_GAIN,&value);
+  *p=((double)value)/65536.0;
+  this->robot_device->read_word_register(HEAD_TRACKING_TILT_I_GAIN,&value);
+  *i=((double)value)/65536.0;
+  this->robot_device->read_word_register(HEAD_TRACKING_TILT_D_GAIN,&value);
+  *d=((double)value)/65536.0;
+  this->robot_device->read_word_register(HEAD_TRACKING_TILT_I_CLAMP,&value);
+  *i_clamp=((double)value)/128.0;
+}
+
+void CDarwinHeadTracking::set_pan_range(double max,double min)
+{
+  unsigned short int value;
+
+  this->is_valid();
+  value=max*128.0;   
+  this->robot_device->write_word_register(HEAD_TRACKING_MAX_PAN,value);
+  value=min*128.0;   
+  this->robot_device->write_word_register(HEAD_TRACKING_MIN_PAN,value);
+}
+
+void CDarwinHeadTracking::get_pan_range(double *max,double *min)
+{
+  unsigned short int value;
+
+  this->is_valid();
+  this->robot_device->read_word_register(HEAD_TRACKING_MAX_PAN,&value);
+  *max=((double)value)/128.0; 
+  this->robot_device->read_word_register(HEAD_TRACKING_MIN_PAN,&value);
+  *min=((double)value)/128.0; 
+}
+
+void CDarwinHeadTracking::set_tilt_range(double max,double min)
+{
+  unsigned short int value;
+
+  this->is_valid();
+  value=max*128.0;  
+  this->robot_device->write_word_register(HEAD_TRACKING_MAX_TILT,value);
+  value=min*128.0;
+  this->robot_device->write_word_register(HEAD_TRACKING_MIN_TILT,value);
+}
+
+void CDarwinHeadTracking::get_tilt_range(double *max,double *min)
+{
+  unsigned short int value;
+
+  this->is_valid();
+  this->robot_device->read_word_register(HEAD_TRACKING_MAX_TILT,&value);
+  *max=((double)value)/128.0;
+  this->robot_device->read_word_register(HEAD_TRACKING_MIN_TILT,&value);
+  *min=((double)value)/128.0;
+}
+
+void CDarwinHeadTracking::start_tracking(double pan,double tilt)
+{
+  unsigned short int value;
+
+  this->is_valid();
+  value=pan*128.0;
+  this->robot_device->write_word_register(HEAD_TRACKING_TARGET_PAN,value);
+  value=tilt*128.0;
+  this->robot_device->write_word_register(HEAD_TRACKING_TARGET_TILT,value);
+  this->robot_device->write_byte_register(HEAD_TRACKING_CONTROL,HEAD_TRACKING_START);
+}
+
+void CDarwinHeadTracking::stop_tracking(void)
+{
+  this->is_valid();
+  this->robot_device->write_byte_register(HEAD_TRACKING_CONTROL,HEAD_TRACKING_STOP);
+}
+
+bool CDarwinHeadTracking::is_tracking(void)
+{ 
+  unsigned char value;
+
+  this->is_valid();
+  this->robot_device->read_byte_register(HEAD_TRACKING_CONTROL,&value);
+  if(value&HEAD_TRACKING_RUNNING)
+    return true;
+  else
+    return false;
+}
+
+void CDarwinHeadTracking::set_new_target(double pan,double tilt)
+{
+  unsigned short int value;
+
+  this->is_valid();
+  value=pan*128.0;
+  this->robot_device->write_word_register(HEAD_TRACKING_TARGET_PAN,value);
+  value=tilt*128.0;
+  this->robot_device->write_word_register(HEAD_TRACKING_TARGET_TILT,value);
+}
+
+void CDarwinHeadTracking::get_current_target(double *pan,double *tilt)
+{
+  unsigned short int value;
+
+  this->is_valid();
+  this->robot_device->read_word_register(HEAD_TRACKING_TARGET_PAN,&value);
+  *pan=((double)value)/128.0;
+  this->robot_device->read_word_register(HEAD_TRACKING_TARGET_TILT,&value);
+  *tilt=((double)value)/128.0;
+}
+
+CDarwinHeadTracking::~CDarwinHeadTracking()
+{
+}
+