PanTiltSoftMotion.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 <cmath>
#include "PanTiltSoftMotion.hh"

using namespace std;
using namespace BIAS;

PanTiltSoftMotion::PanTiltSoftMotion() {
  tilt_active = false;
  min_pan = -2000;
  max_tilt = 2000;
  min_tilt = -2000;
  max_pan = 2000;
  min_pan_speed = 50;
  max_pan_speed = DPPTU_MAXSPEED / 8;
  min_tilt_speed = 50;
  max_tilt_speed = DPPTU_MAXSPEED / 16;
}
PanTiltSoftMotion::~PanTiltSoftMotion() {}

void PanTiltSoftMotion::setLimits(int _min_pan, int _max_pan, int _min_pan_speed, int _max_pan_speed, int _min_tilt_speed, int _max_tilt_speed) {
  min_pan = _min_pan;
  max_pan = _max_pan;
  min_pan_speed = _min_pan_speed;
  max_pan_speed = _max_pan_speed;
  min_tilt_speed = _min_tilt_speed;
  max_tilt_speed = _max_tilt_speed;
}

void PanTiltSoftMotion::assert_pan(int &pan) {
  pan = max(min_pan, pan);
  pan = min(max_pan, pan);
}

void PanTiltSoftMotion::assert_tilt(int &tilt) {
  tilt = max(min_tilt, tilt);
  tilt = min(max_tilt, tilt);
}

int PanTiltSoftMotion::calcSpeed(int start_val, int end_val, int cur_val, int damp) {
  float diff_val = (float)(end_val - start_val);
  //float val = (DPPTU_MAXSPEED / damp) * ((1+cos(2*M_PI*((float)cur_val - (diff_val/2+(float)start_val)) / diff_val)) / 2.0f);
  float val = (DPPTU_MAXSPEED / damp) * cos ( ((2*((float)cur_val-(float)start_val)) / diff_val) -1 );
  return (int) val;
}

int PanTiltSoftMotion::getPanSpeed(int start_pan, int end_pan, int cur_pan, int damp) {
  //assert_pan(start_pan);
  //assert_pan(end_pan);
  int panmod = 1;
  if (end_pan < start_pan) {
    panmod = -1;
    if (cur_pan < end_pan)
      return 0;
  } else {
    if (cur_pan > end_pan)
      return 0;
  }
  int val = calcSpeed(start_pan, end_pan, cur_pan, damp);
  val = max(min_pan_speed,val);
  val = min(max_pan_speed,val);
  val *= panmod;
  return val;
}

int PanTiltSoftMotion::getTiltSpeed(int start_pan, int end_pan, int start_tilt, int end_tilt, int cur_pan, int cur_tilt, int damp) {
  //assert_tilt(start_tilt);
  //assert_tilt(end_tilt);

  int tiltmod = 1;
  if (end_tilt < start_tilt) {
    tiltmod = -1;
    if (cur_tilt < end_tilt) 
      return 0;
  } else {
    if (cur_tilt > end_tilt)
      return 0;
  }
  int val = 0;
  if (start_pan == 0 && end_pan == 0) {
    val = calcSpeed(start_tilt, end_tilt, cur_tilt, damp);
    val = max(min_tilt_speed,val);
    val = min(max_tilt_speed,val);
    val *= tiltmod;
    return val;
  }

  if (abs(cur_pan) > abs(start_pan) && abs(cur_pan) < abs(end_pan)) {
    if ((start_tilt < end_tilt && cur_tilt >= end_tilt) || (start_tilt > end_tilt && cur_tilt <= end_tilt)) {
      tilt_active = false;
    } else {
      tilt_active = true; 
      val = calcSpeed(start_tilt, end_tilt, cur_tilt, damp);
    }
  } else {
    if (tilt_active) {
      val = calcSpeed(start_tilt, end_tilt, cur_tilt, damp);
    }
  }
  if (tilt_active) {
    val = max(min_tilt_speed,val);
    val = min(max_tilt_speed,val);
    val *= tiltmod;
  }
  return val;
}