PanTiltControlInterface.hh

/*
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
*/

#ifndef PanTiltControlInterface_HH_
#define PanTiltControlInterface_HH_

#include <PanTilt/DPPanTiltControl.hh>
#include <math.h>
#include <iostream>

namespace BIAS
{
  /**
     @brief abstract control class for control of Directed Perception Pan Tilt Unit.
   
     Currently uses DPPanTiltControl to send commands but could be extended in the future
     to use any other PTU controller class with common interface.
  
     @author fkellner, ischiller
  */
  class BIASPanTilt_EXPORT PanTiltControlInterface
  {
  public:

    /** @brief Starts controlling the PTU */
    virtual void StartControl() = 0;

    /** @brief Stops/pauses control */
    virtual void StopControl() = 0;

    /** @brief Resumes control */
    virtual void ResumeControl() = 0;

    /** @brief Starts movement to given pan and tilt position */
    virtual void StartMoveTo(const long int pan, const long int tilt) {};

    /** @brief Initialize PTU, called by derived classes.
        @param[in] device  The device string, e.g. COM0 or /dev/ttyUSB0
        @param[n] heavyDutyMode Enables the heavy duty mode for PTU
    */
    PanTiltControlInterface(std::string device, bool heavyDutyMode)
    {
      bool bRet = ptControl.InitPanTiltUnit(device.c_str());
      if (!bRet) {
        std::cout << "PanTiltControlInterface : Initialising PTU failed!" << std::endl;
      }

      // next line sets heavy duty mode
      if (heavyDutyMode) {
        ptControl.SendRawCommand("PU2000 PA2000 PB60 TU2000 TA2000 TB60 PMR TMR %%1R600 DS\n");
      }

      std::string s;
      while (ptControl.ReadRawLine(s) != 0) {
        std::cout << "Read from PTU controller : " << s.c_str() << std::endl;
      }
      ptControl.SetMode(ABSOLUTEMODE);
      await_completion();
      ptControl.SetSpeed(DPPTU_MAXSPEED, DPPTU_MAXSPEED);
      ptControl.SetPosition(0,0);
      await_completion();
      ptControl.GetCurrentPosition(cur_pan, cur_tilt);
      ptControl.GetCurrentPosition(cur_pan, cur_tilt);
      ptControl.QueryResolution(presolution,tresolution);
    }

    /** @brief Default destructor. Moves PTU to position (0,0) and releases interface. */
    virtual ~PanTiltControlInterface()
    {
      ptControl.SetMode(ABSOLUTEMODE);
      ptControl.SetSpeed(DPPTU_MAXSPEED/4, DPPTU_MAXSPEED/4);
      ptControl.SetPosition(0,0);
      await_completion();
      ptControl.ClosePanTiltUnit();
    }

    /** @brief Get position, values should be updated by derived classes. */
    static void GetPosition(long int &pan, long int &tilt)
    {
      pan = cur_pan;
      tilt = cur_tilt;
    }

    /** @brief Get position in degrees, values should be updated by derived classes. */
    static void GetPositionDegrees(double &panDegree, double &tiltDegree)
    {
      panDegree = -cur_pan * presolution * M_PI / 180.0f;
      tiltDegree = cur_tilt * tresolution * M_PI / 180.0f;
    }

    /** @brief Set the position to move to. */
    void SetPosition(long int pan, long int tilt, bool waitComplete = true)
    {
      ptControl.SetMode(ABSOLUTEMODE);
      //ptControl.SetSpeed(DPPTU_MAXSPEED/4, DPPTU_MAXSPEED/4);
      ptControl.SetPosition(pan, tilt, waitComplete);
      cur_pan = pan;
      cur_tilt = tilt;
    }

    /** @brief Waits until the current command is completed. */
    void AwaitCompletion()
    {
      await_completion();
      ptControl.GetCurrentPosition(cur_pan, cur_tilt);
    }

  protected:

    BIAS::DPPanTiltControl ptControl;
    static long int cur_pan;
    static long int cur_tilt;
    static double presolution;
    static double tresolution;
  };

  /**
     @class PanTiltControlFactory
     @brief Abstract controller factory used to create all different kinds of PTU controls.
  */
  class PanTiltControlFactory
  {
  public:
    virtual PanTiltControlInterface* Create(std::string device, bool heavyDutyMode) = 0;
    virtual ~PanTiltControlFactory() {};
  };

}

#endif // PanTiltControlInterface_HH_