DPPanTiltControlExtended.cpp

#include "DPPanTiltControlExtended.hh"
#include "Base/Common/W32Compat.hh"
#include "Base/Common/BIASpragma.hh"
#include <cstring>

using namespace std;
using namespace BIAS;

#ifdef BIAS_HAVE_PTHREADS

pthread_mutex_t DPPanTiltControlExtended::condition_mutex = PTHREAD_MUTEX_INITIALIZER;
long DPPanTiltControlExtended::panThreaded_ = 0;
long DPPanTiltControlExtended::tiltThreaded_ = 0;

#endif

DPPanTiltControlExtended::DPPanTiltControlExtended()
{
  // HACK : Set initial parameters for Point Grey Dragonfly (8mm)
  xApertureAngle_ = 32.97;
  yApertureAngle_ = 25.03;
  xPictureSize_ = 320;
  yPictureSize_ = 240;
  followObjectMaxDiff_ = 400;
  followObjectMinDiff_ = 30;
  xPTSpeed_ = 0;
  yPTSpeed_ = 0;
  xPTDeltaSpeed_ = 0;
  yPTDeltaSpeed_ = 0;
  xPTDeltaSpeedTMinus1_ = 0;
  yPTDeltaSpeedTMinus1_ = 0;

  KiX_ = KiY_= 0.0;
  KdX_ = KdY_ = 0.0;
  KpX_ = KpY_= 1.0;
  ZiNiErrorX_ = 0;
  ZiNiErrorY_ = 0; 
  ZiNiErrorTminus1X_ = 0;
  ZiNiErrorTminus1Y_ = 0;
  RPICounter_ = 0;

  ptuMatrix_.SetZYX(0,0,0);
  deltaMatrix_.SetZYX(0,0,0);
  sensorMatrixMinus1_.SetZYX(0,0,0);
  handEyeMatrix_.SetZYX(0.0, 0.0, 0.0);
  bInitComp_ = true;
  panSpeedComp_ = 0;
  tiltSpeedComp_ = 0; 
}

DPPanTiltControlExtended::~DPPanTiltControlExtended()
{
}

void DPPanTiltControlExtended::SetKp(const BIAS::Vector<int> &kp)
{
  KpX_ = kp[0];
  KpY_ = kp[1];
  KpX_ /= 100;
  KpY_ /= 100;
}

void DPPanTiltControlExtended::SetKi(const BIAS::Vector<int> &ki)
{
  KiX_ = ki[0];
  KiY_ = ki[1];
  KiX_ /= 100; 
  KiY_ /= 100;
}

void DPPanTiltControlExtended::SetKd(const BIAS::Vector<int> &kd)
{
  KdX_ = kd[0];
  KdY_ = kd[1];
  KdX_ /= 100;
  KdY_ /= 100;
}

void DPPanTiltControlExtended::
CompensateHeliMovement(const BIAS::RMatrix &R)
{
  double anglesPTU[3];
  int steps[2];
  long int pan, tilt; 
  BIAS::RMatrix compMatrix;

  if (!bInitComp_)
  {
    GetCurrentPosition(pan, tilt);

    // convert to radians and set
    anglesPTU[2] = ((double)pan * panResolution_)* M_PI/180.0;
    anglesPTU[1] = ((double)tilt * tiltResolution_)* M_PI/180.0;
    anglesPTU[0] = 0.0;
    
    ptuMatrix_.SetZYX(anglesPTU[0],anglesPTU[1],anglesPTU[2]);
    ptuMatrix_.GetRotationAnglesZYX(anglesPTU[0],anglesPTU[1],anglesPTU[2]);

    R.GetRotationAnglesZYX(anglesPTU[0],anglesPTU[1],anglesPTU[2]);
  
    deltaMatrix_ = R * sensorMatrixMinus1_.Transpose();

    sensorMatrixMinus1_.GetRotationAnglesZYX(anglesPTU[0],anglesPTU[1],anglesPTU[2]);

    deltaMatrix_.GetRotationAnglesZYX(anglesPTU[0],anglesPTU[1],anglesPTU[2]);
    
    compMatrix = ptuMatrix_ * handEyeMatrix_ * deltaMatrix_ *
                 handEyeMatrix_.Transpose() * ptuMatrix_.Transpose();
  
    compMatrix.GetRotationAnglesZYX(anglesPTU[0],anglesPTU[1],anglesPTU[2]);

    steps[0] = (int)(anglesPTU[2] * 180.0/M_PI / panResolution_);
    steps[1] = (int)(anglesPTU[1] * 180.0/M_PI / tiltResolution_);
    
    panSpeedComp_ = (short int)(steps[0]/T_*1e6);
    tiltSpeedComp_ =(short int)(steps[1]/T_*1e6);
    if (!bDefaultOrientation_) {
      panSpeedComp_ = -panSpeedComp_;
      tiltSpeedComp_ = tiltSpeedComp_;
    } else {
      panSpeedComp_ = panSpeedComp_;
      tiltSpeedComp_ = tiltSpeedComp_;
    }
  } else {
    bInitComp_ = false;
  }

  // save the given compensation matrix as last sensor matrix (t-1)
  sensorMatrixMinus1_ = R;
}

#ifdef BIAS_HAVE_PTHREADS

void DPPanTiltControlExtended::StartCircularMovementThread()
{
  pthread_create(&control_thread, NULL, this->threadFuncCircle_, this);
}

void DPPanTiltControlExtended::
GetCurrentPositionDegreesThreaded(double &dPanDeg, double& dTiltDeg)
{
  pthread_mutex_lock(&condition_mutex); 
  dPanDeg =  panThreaded_ * panResolution_;
  dTiltDeg = tiltThreaded_ * tiltResolution_;
  pthread_mutex_unlock(&condition_mutex); 
}

void DPPanTiltControlExtended::
GetCurrentPositionRadiansThreaded(double &dPanRad, double& dTiltRad)
{
  pthread_mutex_lock(&condition_mutex);
  dPanRad =  panThreaded_ * panResolution_ * M_PI / 180.0;
  dTiltRad = tiltThreaded_ * tiltResolution_ * M_PI / 180.0;
  pthread_mutex_unlock(&condition_mutex);
}

void* DPPanTiltControlExtended::threadFuncCircle_(void *ptr)
{
  DPPanTiltControlExtended *ptc = (DPPanTiltControlExtended*)ptr;
  ptc->SetMode(ABSOLUTEMODE);
  ptc->SetSpeed(50,50);
  // first move to zero
  ptc->SetPosition(0,0);
  await_completion();
  // move up
  ptc->SetPosition(0,200);
  pthread_mutex_lock(&condition_mutex); 
  ptc->GetCurrentPosition(panThreaded_,tiltThreaded_);
  pthread_mutex_unlock(&condition_mutex);
  while(panThreaded_ != 0 && tiltThreaded_ != 200){
    biasusleep(100);
    pthread_mutex_lock(&condition_mutex); 
    ptc->GetCurrentPosition(panThreaded_,tiltThreaded_);
    pthread_mutex_unlock(&condition_mutex);
  }
  // move to upper left
  ptc->SetPosition(200,200);
  while(panThreaded_ != 200 && tiltThreaded_ != 200){
    biasusleep(100);
    pthread_mutex_lock(&condition_mutex); 
    ptc->GetCurrentPosition(panThreaded_,tiltThreaded_);
    pthread_mutex_unlock(&condition_mutex);
  }
  // move to lower left
  ptc->SetPosition(200,-200);
  while(panThreaded_ != 200 && tiltThreaded_ != -200){
    biasusleep(100);
    pthread_mutex_lock(&condition_mutex); 
    ptc->GetCurrentPosition(panThreaded_,tiltThreaded_);
    pthread_mutex_unlock(&condition_mutex);
  }
  // lower right
  ptc->SetPosition(-200,-200);
  while(panThreaded_ != -200 && tiltThreaded_ != -200){
    biasusleep(100);
    pthread_mutex_lock(&condition_mutex); 
    ptc->GetCurrentPosition(panThreaded_,tiltThreaded_);
    pthread_mutex_unlock(&condition_mutex);
  }
  // move to upper right
  ptc->SetPosition(-200,200);
  while(panThreaded_ != -200 && tiltThreaded_ != 200){
    biasusleep(100);
    pthread_mutex_lock(&condition_mutex); 
    ptc->GetCurrentPosition(panThreaded_,tiltThreaded_);
    pthread_mutex_unlock(&condition_mutex);
  }
  // move up
  ptc->SetPosition(0,200);
  while(panThreaded_ != 0 && tiltThreaded_ != 200){
    biasusleep(100);
    pthread_mutex_lock(&condition_mutex); 
    ptc->GetCurrentPosition(panThreaded_,tiltThreaded_);
    pthread_mutex_unlock(&condition_mutex);
  }
  // reset position to zero
  ptc->SetPosition(0,0);
  while(panThreaded_ != 0 && tiltThreaded_ != 0){
    biasusleep(100);
    pthread_mutex_lock(&condition_mutex); 
    ptc->GetCurrentPosition(panThreaded_,tiltThreaded_);
    pthread_mutex_unlock(&condition_mutex);
  }
  return NULL;
}

#endif // BIAS_HAVE_PTHREADS

void DPPanTiltControlExtended::
FollowMidPos(const BIAS::Vector2<double> &newMean)
{  
  // distance on the image plane
  int d[2];
  d[0] = -((int)xPictureSize_/2)+(int)newMean[0];
  d[1] = -((int)yPictureSize_/2)+(int)newMean[1];

  // the angle which the camera has to turn  
  float beta[2];
  beta[0] = ((float)d[0]/(float)xPictureSize_) * xApertureAngle_;
  beta[1] = ((float)d[1]/(float)yPictureSize_) * yApertureAngle_;
  
  // now calculate positions from angle
  int stepsPerDegreePan = (int)(1.0/panResolution_);
  int stepsPerDegreeTilt = (int)(1.0/tiltResolution_);
  int steps[2];
  steps[0] = (int)(beta[0] * stepsPerDegreePan);
  steps[1] = (int)(beta[1] * stepsPerDegreeTilt);

  // switch sign
  if (bDefaultOrientation_) {
    steps[0] = -steps[0]; 
  } else {
    //steps[1] = -steps[1];
  }
 
  if (steps[0] > followObjectMinDiff_ && steps[0] < followObjectMaxDiff_)
  {
    set_desired(PAN, POSITION, (PTU_PARM_PTR *) &steps[0],RELATIVE);
    await_completion();
    //cout << "DPPanTiltControlExtended : Now following mid pos, steps x = " << steps[0] << endl;
  }
  if (steps[1] > followObjectMinDiff_ && steps[1] < followObjectMaxDiff_)
  {
    set_desired(TILT, POSITION, (PTU_PARM_PTR *) &steps[1],RELATIVE);
    await_completion();
    //cout << "DPPanTiltControlExtended : Now following mid pos, steps y = " << steps[1] << endl;
  }
}

void DPPanTiltControlExtended::
PIDSpeedAdjustFollow(const BIAS::Vector2<double> &newMean)
{
  // distance on the image plane
  double error[2];
  error[0] = -((double)xPictureSize_/2)+newMean[0];
  error[1] = -((double)yPictureSize_/2)+newMean[1];

  ZiNiErrorX_+=(int)error[0];
  ZiNiErrorY_+=(int)error[1];
  
  double maxSpeedScaleX = (error[0]/xPictureSize_);
  double maxSpeedScaleY = (error[1]/yPictureSize_);
  
  // now calculate new speeds
  xPTSpeed_ = (short int)(maxSpeed_ * maxSpeedScaleX
                          * KpX_); // * error[0]); // / xPictureSize_;
  yPTSpeed_ = (short int)(maxSpeed_ * maxSpeedScaleY 
                          * KpY_); // * error[1]); // / yPictureSize_;

  // add the integrative part to the speed (sum of all errors over time)
  xPTSpeed_ +=  (short int)KiX_ * ZiNiErrorX_;
  yPTSpeed_ +=  (short int)KiY_ * ZiNiErrorY_;

  // add the derivative part to the speed (d e(t)/dt)
  xPTSpeed_ +=  (short int)(KdX_ * (error[0]- ZiNiErrorTminus1X_));
  yPTSpeed_ +=  (short int)(KdY_ * (error[1]- ZiNiErrorTminus1Y_));
  
  // now save actual error as t-1 error
  ZiNiErrorTminus1X_=(int)error[0];
  ZiNiErrorTminus1Y_=(int)error[1];

  // switch speeds if camera upside down
  if(bDefaultOrientation_) {
    xPTSpeed_ = -xPTSpeed_;
  } else {
    //yPTSpeed_ = -yPTSpeed_;
  }
  // add the speeds from the helicopter movement compensation calculation
  xPTSpeed_ += panSpeedComp_;
  yPTSpeed_ += tiltSpeedComp_;

  // and set the new speeds
  set_desired(PAN, SPEED, (PTU_PARM_PTR *) &xPTSpeed_, ABSOLUTE);
  set_desired(TILT, SPEED, (PTU_PARM_PTR *) &yPTSpeed_, ABSOLUTE);

  cout << " DPPanTiltControlExtended : Set speed to " << xPTSpeed_ << ", " << yPTSpeed_ << endl;
}

void DPPanTiltControlExtended::
RecursivePIControl(const BIAS::Vector2<double> &newMean)
{
  // distance on the image plane
  int error[2];
  error[0] = (int)(newMean[0]-xPictureSize_/2);
  error[1] = (int)(newMean[1]-yPictureSize_/2);

  if(RPICounter_ == 0)
  {
    // u(0) (e(k-1) and u(k-1) are zero here)
    // scale maxspeed with picturesize / error = speed relative to error
    xPTDeltaSpeed_=(short int)(KpX_*(1+T_/(2*Tn_))*error[0]);
    yPTDeltaSpeed_=(short int)(KpY_*(1+T_/(2*Tn_))*error[1]);
    
    // switch speeds if camera upside down
    if(bDefaultOrientation_)
      xPTDeltaSpeed_ = -xPTDeltaSpeed_;
    else
      yPTDeltaSpeed_ = -yPTDeltaSpeed_;

    // and set the new speeds
    set_desired(PAN,SPEED,(PTU_PARM_PTR *) &xPTDeltaSpeed_, RELATIVE);
    set_desired(TILT,SPEED,(PTU_PARM_PTR *) &yPTDeltaSpeed_, RELATIVE);

    // save the speed for next iteration
    xPTDeltaSpeedTMinus1_ = xPTDeltaSpeed_;
    yPTDeltaSpeedTMinus1_ = yPTDeltaSpeed_;

    // save the error here
    ErrorTMinus1_.push_back(error[0]);
    ErrorTMinus1_.push_back(error[1]);

    RPICounter_++;
  }    
  else
  {
    // u(1) (u(k-2) is zero here)
    xPTDeltaSpeed_ = (short int)(KpX_*(1+(T_/(2*Tn_)))
                                 *error[0]+KpX_*(-1+(T_/(2*Tn_)))*ErrorTMinus1_[0]
                                 +0.1*xPTDeltaSpeedTMinus1_);
         
    yPTDeltaSpeed_ = (short int)(KpY_*(1+(T_/(2*Tn_)))
                                 *error[1]+KpY_*(-1+(T_/(2*Tn_)))*ErrorTMinus1_[1]
                                 +yPTDeltaSpeedTMinus1_);

    
    // switch speeds if camera upside down
    if(bDefaultOrientation_)
      xPTDeltaSpeed_ = -xPTDeltaSpeed_;
    else
      yPTDeltaSpeed_ = -yPTDeltaSpeed_;
    
    // and set the new speeds
    set_desired(PAN,SPEED,(PTU_PARM_PTR *) &xPTDeltaSpeed_, RELATIVE);
    set_desired(TILT,SPEED,(PTU_PARM_PTR *) &yPTDeltaSpeed_, RELATIVE);

    // now save speeds and errors for the next iteration
    xPTDeltaSpeedTMinus1_ = xPTDeltaSpeed_;
    yPTDeltaSpeedTMinus1_ = yPTDeltaSpeed_;
  
    ErrorTMinus1_[0] = error[0];
    ErrorTMinus1_[1] = error[1];
  }
}