PanTiltAutoControl.cpp

/*
This file is part of the BIAS library (Basic ImageAlgorithmS).

Copyright (C) 2003-2009    (see file CONTACT for details)
  Multimediale Systeme der Informationsverarbeitung
  Institut fuer Informatik
  Christian-Albrechts-Universitaet Kiel


BIAS is free software; you can redistribute it and/or modify
it under the terms of the GNU Lesser General Public License as published by
the Free Software Foundation; either version 2.1 of the License, or
(at your option) any later version.

BIAS is distributed in the hope that it will be useful,
but WITHOUT ANY WARRANTY; without even the implied warranty of
MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
GNU Lesser General Public License for more details.

You should have received a copy of the GNU Lesser General Public License
along with BIAS; if not, write to the Free Software
Foundation, Inc., 59 Temple Place, Suite 330, Boston, MA  02111-1307  USA
*/

#include <Base/Common/BIASpragma.hh>
#include <PanTilt/DPPanTiltControl.hh>
#include <pthread.h>

#include "PanTiltAutoControl.hh"
#include "PanTiltSoftMotion.hh"

using namespace std;
using namespace BIAS;

bool PanTiltAutoControl::waiting_;
bool PanTiltAutoControl::exiting_;
bool PanTiltAutoControl::started_;
bool PanTiltAutoControl::bSpeedMode_;
long int PanTiltAutoControl::des_pan;
long int PanTiltAutoControl::des_tilt;
pthread_mutex_t PanTiltAutoControl::condition_mutex = PTHREAD_MUTEX_INITIALIZER;
pthread_cond_t PanTiltAutoControl::condition_cond = PTHREAD_COND_INITIALIZER;


PanTiltAutoControl::PanTiltAutoControl(std::string device, bool heavyDutyMode):
  PanTiltControlInterface(device, heavyDutyMode)
{
  started_ = false;
  bSpeedMode_ = false;
}

PanTiltAutoControl::~PanTiltAutoControl() {
  if (waiting_)
    ResumeControl();
  if (started_) {
    exiting_ = true;
    while (started_)
      pthread_cond_wait( &condition_cond, &condition_mutex );
  }
  ptControl.ClosePanTiltUnit();
}

void PanTiltAutoControl::StartControl() {
  if (started_) return;

  started_ = true;
  waiting_ = false;
  exiting_ = false;
  ptControl.GetCurrentPosition(cur_pan, cur_tilt);
  cout << "current pos: " << cur_pan << " " << cur_tilt << endl;
  ptControl.SetMode(SPEEDMODE);
  await_completion();
  ptControl.SetSpeed(0,0);
  await_completion();
  ptControl.GetCurrentPosition(cur_pan, cur_tilt);
  bSpeedMode_ = true;
  cout << "current pos: " << cur_pan << " " << cur_tilt << endl;
  pthread_create(&control_thread, NULL, this->threadFunc_pan_tilt, &ptControl); 
}

void PanTiltAutoControl::StopControl() {
  pthread_mutex_lock( &condition_mutex );
  waiting_ = true;
  ptControl.SetSpeed(0,0);
  pthread_mutex_unlock( &condition_mutex );
}

void PanTiltAutoControl::ResumeControl() {
  pthread_mutex_lock( &condition_mutex );
  waiting_ = false;
  pthread_cond_signal( &condition_cond );
  pthread_mutex_unlock( &condition_mutex );
}

void PanTiltAutoControl::StartMoveTo(const long int pan, const long int tilt) {
  if (!bSpeedMode_) {
    ptControl.GetCurrentPosition(cur_pan, cur_tilt);
    ptControl.SetMode(SPEEDMODE);
    ptControl.SetSpeed(0,0);
    ptControl.GetCurrentPosition(cur_pan, cur_tilt);
    bSpeedMode_ = true;
  }
  // if moving new command will be skipped, use stopMoveTo() to stop first
  if (started_ )  return;
  started_ = true;
  des_pan = pan;
  des_tilt = tilt;
  pthread_create(&control_thread, NULL, this->threadFunc_move_to, &ptControl);
}

void PanTiltAutoControl::StopMoveTo() {
  pthread_mutex_lock( &condition_mutex );
  if (started_) {
    exiting_ = true;
    while (started_)
      pthread_cond_wait( &condition_cond, &condition_mutex );
  }
  pthread_mutex_unlock( &condition_mutex );
}

void* PanTiltAutoControl::threadFunc_pan_tilt(void *ptr)
{
  DPPanTiltControl *ptc = (DPPanTiltControl*)ptr;
  t_SoftMotion sm;
  float speed_damp = 32.0f; 
  //sm.setLimits(-2000,2000,100,DPPTU_MAXSPEED/speed_damp,50,DPPTU_MAXSPEED/speed_damp);
  int k=0;
  int pspeed = 0;
  int tspeed = 0;
  int start_pan=0, start_tilt=0, end_pan=0, end_tilt=0, tilt_in_start=0, tilt_in_end=0;
  bool loop = true;
  while (loop) {
    ptc->GetCurrentPosition(cur_pan, cur_tilt);
    pthread_mutex_lock( &condition_mutex );
    while (waiting_ && !exiting_)
      pthread_cond_wait( &condition_cond, &condition_mutex );
    pthread_mutex_unlock( &condition_mutex );
    
    if (exiting_) {
      loop = false;
      ptc->SetSpeed(0,0);
      await_completion();
      started_ = false;
      pthread_cond_signal( &condition_cond );
      break;
    }

    int pmod = 1;
    tilt_in_start = -600; tilt_in_end = -1000;
    if (k%2==0) {
      pmod = -1;
      tilt_in_start = 600; tilt_in_end = 1000;
    } 
    start_pan = -1*pmod*1000; end_pan = pmod*1000;
    switch (k) {
      case 0: 
        start_pan = 0; end_pan = -1000;
        start_tilt = 0; end_tilt = -400;
        tilt_in_start = 0; tilt_in_end = -1000;
        break;
      case 2:
        start_tilt = -400; end_tilt = -200;
        break;
      case 3:
        start_tilt = -200; end_tilt = 0;
        break;
      case 4:
        start_tilt = 0; end_tilt = 200;
        break;
      case 5:
        start_tilt = 200; end_tilt = 400;
        break;
      case 6:
        start_tilt = 400; end_tilt = 600;
        break;
      case 7:
        start_tilt = 600; end_tilt = 800;
        break;
      case 8:
        //ptc->SetMode(ABSOLUTEMODE);
        //ptc->SetPosition(0,0);
        //k = 0;
        exiting_ = true;
        continue;
        break;
    }

    pspeed = sm.getPanSpeed(start_pan,end_pan,cur_pan,speed_damp);  
    tspeed = sm.getTiltSpeed(tilt_in_start,tilt_in_end,start_tilt,end_tilt,cur_pan,cur_tilt,speed_damp);
    ptc->GetCurrentPosition(cur_pan, cur_tilt);
    pthread_mutex_lock( &condition_mutex );
    if (!waiting_) {
      ptc->SetSpeed(pspeed,tspeed);
    }
    pthread_mutex_unlock( &condition_mutex );
    if ( (pmod < 0 && cur_pan <= end_pan)  ||( pmod > 0 && cur_pan >= end_pan)) {
      k++;
    }
    ptc->GetCurrentPosition(cur_pan, cur_tilt);
  }
  return 0;
}

void* PanTiltAutoControl::threadFunc_pan_only(void *ptr) {
  DPPanTiltControl *ptc = (DPPanTiltControl*)ptr;
  int min_pan = -2000;
  int max_pan = 2000;
  int pmod = 1;
    
  ptc->GetCurrentPosition(cur_pan, cur_tilt);
  while (1) {
    pthread_mutex_lock( &condition_mutex );
    while (waiting_ && !exiting_)
      pthread_cond_wait( &condition_cond, &condition_mutex );
    pthread_mutex_unlock( &condition_mutex );

    if (exiting_) {
      ptc->SetSpeed(0,0);
      await_completion();
      break;
    }
    if (cur_pan >=max_pan) 
      pmod = -1;
    if (cur_pan <=min_pan)
      pmod = 1;
        
    int pspeed = 0;
    float speedmod = (1+cos((float)cur_pan*M_PI / 2000.0f)) / 2.0f;
    pspeed = abs(DPPTU_MAXSPEED*speedmod) / 4;      
    pspeed = max(100,pspeed);
    pspeed = min(DPPTU_MAXSPEED/2,pspeed);
    pspeed *= pmod;
    //pspeed = pmod*(DPPTU_MAXSPEED/4);
    pthread_mutex_lock( &condition_mutex );
    if (!waiting_)
      ptc->SetSpeed(pspeed, 0);
    pthread_mutex_unlock( &condition_mutex );
    ptc->GetCurrentPosition(cur_pan, cur_tilt);
    cout << cur_pan << " " << cur_tilt << endl;
  }
  return 0;
}

void* PanTiltAutoControl::threadFunc_move_to(void *ptr) {
  exiting_ = false;
  DPPanTiltControl *ptc = (DPPanTiltControl*)ptr;
  t_SoftMotion sm;
  //sm.setLimits(-2000,2000,100,DPPTU_MAXSPEED/2,50,DPPTU_MAXSPEED/2);
  long int start_pan, start_tilt;
  int pspeed = 0, tspeed = 0;
  ptc->GetCurrentPosition(start_pan, start_tilt);
  while (1) {
    ptc->GetCurrentPosition(cur_pan, cur_tilt);
    if (abs(des_pan - cur_pan) < 10 && abs(des_tilt - cur_tilt) < 10)
      exiting_ = true;
    if (exiting_) {
      ptc->SetSpeed(0,0);
      await_completion();
      started_ = false;
      pthread_cond_signal( &condition_cond );
      break;
    }
    pspeed = sm.getPanSpeed(start_pan, des_pan, cur_pan, 4);
    tspeed = sm.getTiltSpeed(0, 0, start_tilt, des_tilt, cur_pan, cur_tilt, 8);
    pthread_mutex_lock( &condition_mutex );
    if (!waiting_)
      ptc->SetSpeed(pspeed,tspeed);
    pthread_mutex_unlock( &condition_mutex );
    if (pspeed == 0 && tspeed == 0)
      exiting_ = true;
    ptc->GetCurrentPosition(cur_pan, cur_tilt);
  }
  return 0;
}

int PanTiltAutoControl::abs(int val) {
  return (val < 0) ? -val : val;
}

int PanTiltAutoControl::min(int val1, int val2) {
  return (val1 < val2) ? val1 : val2;
}

int PanTiltAutoControl::max(int val1, int val2) {
  return (val1 < val2) ? val2 : val1;
}