DPPanTiltControlExtended.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 __DPPanTiltControlExtended_hh__
#define __DPPanTiltControlExtended_hh__

#include "DPPanTiltControl.hh"

#include <Base/Math/Vector.hh>
#include <Base/Math/Vector2.hh>
#include <Geometry/RMatrix.hh>

#ifdef BIAS_HAVE_PTHREADS
#  include <pthread.h>
#endif

// define object tracking control mode
#define TRACK_ABS_POS_MODE       100
#define TRACK_PID_SPEED_MODE     101
#define TRACK_REC_SPEED_MODE     102

namespace BIAS
{
  /**
     @class DPPanTiltControlExtended
     @brief Extended controller interface for Directed Perception Pan Tilt Unit.
     @author Ingo Schiller
     @data 10/2011
  */ 
  class BIASPanTilt_EXPORT DPPanTiltControlExtended : public DPPanTiltControl
  {
  public:

    /** @brief Standard constructor */
    DPPanTiltControlExtended();

    /** @brief Standard destructor */
    ~DPPanTiltControlExtended();

    /** @brief Set the proportional parameter of the Ziegler-Nichols tuning. */
    void SetKp(const BIAS::Vector<int> &kp);

    /** @brief Set the integral parameter of the Ziegler-Nichols tuning. */
    void SetKi(const BIAS::Vector<int> &ki);

    /** @brief Set the derivative parameter of the Ziegler-Nichols tuning. */
    void SetKd(const BIAS::Vector<int> &kd);

    /**@brief Compensates movement of coordinate system the PTU is mounted to.
       @attention Only rotation is supported yet!
       @note Implemented for helicopter project by Ingo Schiller */
    void CompensateHeliMovement(const BIAS::RMatrix &R);
    
    /**@brief Simple object tracking method, uses absolut positioning error
              and moves PTU and camera to position newMean.
       @param[in] newMean Defines the setpoint to adjust the PTU to.
       @attention Mode has to be set to ABSOLUTEMODE! */
    void FollowMidPos(const BIAS::Vector2<double> &newMean);
  
    /** @brief Perform PID controlling of the PTU. Very simple algorithm
               without deadtime and frequency.
        @param[in] newMean Defines the setpoint to adjust the PTU to.
        @attention Mode has to be set to SPEEDMODE! */
    void PIDSpeedAdjustFollow(const BIAS::Vector2<double> &newMean);
 
    /**@brief Perform digital controlling of the PTU as described in Herbert
              Schlitt (pages 215 ff.).
       Takes care of deadtime and frequency of the algorithm. Makes recursive
       calculation and calculates the difference in  velocity of the PTU.
       @param[in] newMean Defines the setpoint to adjust the PTU to.
       @attention Mode has to be set to SPEEDMODE! */
    void RecursivePIControl(const BIAS::Vector2<double> &newMean);

    inline void SetSamplingTime(float t) { T_ = t; }

    inline void SetResetTime(float tn) { Tn_ = tn; }

    /** @brief Set the size of the picture of the camera mounted to the PTU.
               This is needed to calculate the speeds to keep an object in
               the middle of the picture while steering the PTU.
        @note Implemented for helicopter project by Ingo Schiller */
    inline void SetPictureSize(unsigned int xSize, unsigned int ySize) {
      xPictureSize_ = xSize;
      yPictureSize_ = ySize;
    }

    /** @brief Set the aperture angle of the camera mounted to the PTU.
               This is needed to calculate the speeds to keep an object in
               the middle of the picture while steering the PTU.
        @note Implemented for helicopter project by Ingo Schiller */
    inline void SetApertureAngle(double xAngle, double yAngle) {
      xApertureAngle_ = xAngle;
      yApertureAngle_ = yAngle;
    }

#ifdef BIAS_HAVE_PTHREADS

    /** @brief Start a thread moving the PTU in a circle. */
    void StartCircularMovementThread();

    /** @brief Get the position in degrees while moving in thread. */
    void GetCurrentPositionDegreesThreaded(double &dPanDeg, double& dTiltDeg);

    /** @brief Get the position in radians while moving in thread. */
    void GetCurrentPositionRadiansThreaded(double &dPanRad, double& dTiltRad);

#endif

  protected:

#ifdef BIAS_HAVE_PTHREADS
    
    // variables for thread to control camera movement
    pthread_t control_thread; 
    static pthread_mutex_t condition_mutex;
    static long panThreaded_;
    static long tiltThreaded_;
    static void* threadFuncCircle_(void *ptr);

#endif

    // parameters for Ziegler-Nichols tuning
    double KpX_, KpY_, KiX_, KiY_, KdX_, KdY_;
    int ZiNiErrorX_; // total error in the Ziegler-Nichols tuning method  x
    int ZiNiErrorY_; //  total error in the Ziegler-Nichols tuning method  y
    int ZiNiErrorTminus1X_;
    int ZiNiErrorTminus1Y_;

    // variables for object tracking
    int followObjectMaxDiff_;
    int followObjectMinDiff_;
    double xApertureAngle_;
    double yApertureAngle_;
    unsigned int xPictureSize_;
    unsigned int yPictureSize_;

    // variables for sensor/motion compensation
    bool  bInitComp_;
    short int xPTSpeed_;
    short int yPTSpeed_; 
    short int panSpeedComp_;
    short int tiltSpeedComp_;

    // orientation matrices for sensor/motion compensation
    BIAS::RMatrix ptuMatrix_, deltaMatrix_, handEyeMatrix_, sensorMatrixMinus1_;
    
    // variables for recursive control
    float T_;  // time between discretisation of sampler 
    float Tn_; // time lag
    short int xPTDeltaSpeed_;  
    short int yPTDeltaSpeed_;
    short int xPTDeltaSpeedTMinus1_;  
    short int yPTDeltaSpeedTMinus1_;
    std::vector<int> ErrorTMinus1_;
    int RPICounter_; // iteration counter for recursive control

  };

}

#endif // __DPPanTiltControlExtended_hh__