tibi_dabo_head_tracking_alg.cpp

Go to the documentation of this file.

#include "tibi_dabo_head_tracking_alg.h"

TibiDaboHeadTrackingAlgorithm::TibiDaboHeadTrackingAlgorithm(void)
{
  this->current_pan=0.0;
  this->current_tilt=0.0;

  this->pan_control.P=0.01;
  this->pan_control.I=0.0;
  this->pan_control.D=0.0;
  this->tilt_control.P=0.01;
  this->tilt_control.I=0.0;
  this->tilt_control.D=0.0;

  this->max_pan_angle=150.0*3.14159/180.0;
  this->min_pan_angle=-150.0*3.14159/180.0;
  this->max_tilt_angle=125.0*3.14159/180.0;
  this->min_tilt_angle=-125.0*3.14159/180.0;

  this->pan_error=0.0;
  this->tilt_error=0.0;
}

TibiDaboHeadTrackingAlgorithm::~TibiDaboHeadTrackingAlgorithm(void)
{
}

void TibiDaboHeadTrackingAlgorithm::config_update(Config& new_cfg, uint32_t level)
{
  this->lock();

  // save the current configuration
  this->pan_control.P=new_cfg.pan_P;
  this->pan_control.I=new_cfg.pan_I;
  this->pan_control.D=new_cfg.pan_D;
  this->tilt_control.P=new_cfg.tilt_P;
  this->tilt_control.I=new_cfg.tilt_I;
  this->tilt_control.D=new_cfg.tilt_D;

  this->max_pan_angle=new_cfg.max_pan;
  this->min_pan_angle=new_cfg.min_pan;
  this->max_tilt_angle=new_cfg.max_tilt;
  this->min_tilt_angle=new_cfg.min_tilt;
  this->config_=new_cfg;
  
  this->unlock();
}

// TibiDaboHeadTrackingAlgorithm Public API
void TibiDaboHeadTrackingAlgorithm::compute_head_position(float target_pan,float target_tilt,float *pan, float *tilt)
{
  static float last_pan_error=0.0,int_pan_error=0.0;
  static float last_tilt_error=0.0,int_tilt_error=0.0;

  this->lock();
  this->pan_error=target_pan-this->current_pan;
  this->tilt_error=target_tilt-this->current_tilt;
  (*pan)=(pan_error*this->pan_control.P)+(pan_error-last_pan_error)*this->pan_control.D-int_pan_error*this->pan_control.I+this->current_pan;
  if((*pan)>this->max_pan_angle)
    (*pan)=this->max_pan_angle;
  else if((*pan)<this->min_pan_angle)
    (*pan)=this->min_pan_angle;
  (*tilt)=(tilt_error*this->tilt_control.P)+(tilt_error-last_tilt_error)*this->tilt_control.D-int_tilt_error*this->tilt_control.I+this->current_tilt;
  if((*tilt)>this->max_tilt_angle)
    (*tilt)=this->max_tilt_angle;
  else if((*tilt)<this->min_tilt_angle)
    (*tilt)=this->min_tilt_angle;
  last_pan_error=pan_error;
  last_tilt_error=tilt_error;
  int_pan_error+=pan_error;
  int_tilt_error+=tilt_error;
  this->unlock();
}

void TibiDaboHeadTrackingAlgorithm::set_current_head_position(float pan, float tilt)
{
  this->lock();
  this->current_pan=pan;
  this->current_tilt=tilt;
  this->unlock();
}

void TibiDaboHeadTrackingAlgorithm::get_current_error(float *pan_error,float *tilt_error)
{
  (*pan_error)=this->pan_error;
  (*tilt_error)=this->tilt_error;
}