robotflow-cvs

[Fernyqc <fe...@us...>]

Update of /cvsroot/robotflow/RobotFlow/Behaviors/include
In directory sc8-pr-cvs1.sourceforge.net:/tmp/cvs-serv21067/Behaviors/include

Added Files:
	AvoidUtil.h 
Log Message:
Header file use in EllipticalAvoid

--- NEW FILE: AvoidUtil.h ---
#ifndef AVOID_UTIL_H
#define AVOID_UTIL_H

#include <vector>
#include <exception>

// Exception classes
class AvoidUtilInvalidConstructorArguments: public std::exception
{
  virtual const char* what() const throw();
};

class AvoidUtilIllegalAvoidingThreshold: public std::exception
{
  virtual const char* what() const throw();
};

class AvoidUtilFindSmallestMarginException: public std::exception
{
  virtual const char* what() const throw();
};

class AvoidUtilBeltPositionOutOfRange: public std::exception
{
  virtual const char* what() const throw();
};

class AvoidUtilMarginException: public std::exception
{
  virtual const char* what() const throw();
};

class AvoidUtil
{
   public:
      AvoidUtil(unsigned int maxVelocity, 
                unsigned int minEllipseLong, 
                unsigned int ellipseLong, 
                unsigned int ellipseLat,
                double zeroVelSecurityMargin,
                double reverseVelSecurityMargin,
                int reverseVel,
                double rotVelSecurityMargin,
                int rotVel);
      ~AvoidUtil();
      unsigned int evalLongAvoidingThres(int velocity) const;
      unsigned int evalEllipticalAvoidingThres(unsigned int theta, unsigned int distanceThres) const;
      bool createEllipticalAvoidingThresBelt(unsigned int beltSize, unsigned int distanceThres);
      const std::vector<int>& getEllipticalAvoidingThresBelt();
      bool createProximityMarginBelt(std::vector<int> rangeBelt);
      const std::vector<double>& getProximityMarginBelt();
      void findSmallestProximityMarginAndPosition(unsigned int lPos, unsigned int hPos, double& margin, unsigned int& pos);
      bool isAvoidanceNeeded(std::vector<int> rangeBelt, int velocity, unsigned int &pos, double &margin);
      int evalRotationalDirection(int velocity, unsigned int position);
      //unsigned int evalEllipticalDirectionChangeThres(unsigned int theta) const;
     // bool createEllipticalDirectionChangeThresBelt(unsigned int beltSize);
      //const std::vector<unsigned int>& getEllipticalDirectionChangeThresBelt();
      //bool isDirectionChangeNeeded(unsigned int position, unsigned int rangeValue);
      int evalAvoidingLinVel(int velocity, double margin);
      int evalAvoidingRotVel(int velocity, unsigned int pos, double margin);
   
   private:
      // Function
      unsigned int evalEllipticalThres(unsigned int theta, unsigned int distanceThres, unsigned int ellipseLong, unsigned int ellipseLat) const;
      
      // Constructor arguments
      unsigned int m_maxVelocity;   
      unsigned int m_minEllipseLong;
      unsigned int m_ellipseLong;
      unsigned int m_ellipseLat;
      double m_zeroVelSecurityMargin;
      double m_reverseVelSecurityMargin;
      int m_reverseVel;
      double m_rotVelSecurityMargin;
      int m_rotVel; 
      
      // Variables
      double m_slope;
      double m_reverseSlope;
      double m_reverseB;
      double m_slopeRot;
      double m_rotB;
      std::vector<int> m_ellipseBelt;
      std::vector<double> m_proximityMarginBelt;
      //std::vector<int> m_directionChangeBelt;
};

#endif