[Robotflow-cvs] RobotFlow/Behaviors/src AvoidUtil.cc,1.2,1.3

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

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

Modified Files:
	AvoidUtil.cc 
Log Message:
Elimination of the oscillation when detecting obstacle at front

Index: AvoidUtil.cc
===================================================================
RCS file: /cvsroot/robotflow/RobotFlow/Behaviors/src/AvoidUtil.cc,v
retrieving revision 1.2
retrieving revision 1.3
diff -C2 -d -r1.2 -r1.3
*** AvoidUtil.cc	13 Dec 2005 14:47:17 -0000	1.2
--- AvoidUtil.cc	20 Dec 2005 21:22:42 -0000	1.3
***************
*** 36,40 ****
                       int reverseVel,
                       double rotVelSecurityMargin,
!                      int rotVel) :
                       m_typVelocity(typVelocity), 
                       m_minEllipseLong(minEllipseLong), 
--- 36,41 ----
                       int reverseVel,
                       double rotVelSecurityMargin,
!                      int rotVel,
!                      unsigned int nbOscillation) :
                       m_typVelocity(typVelocity), 
                       m_minEllipseLong(minEllipseLong), 
***************
*** 45,49 ****
                       m_reverseVel(reverseVel),
                       m_rotVelSecurityMargin(rotVelSecurityMargin),
!                      m_rotVel(rotVel)
  {
     // The velocity must not be o
--- 46,51 ----
                       m_reverseVel(reverseVel),
                       m_rotVelSecurityMargin(rotVelSecurityMargin),
!                      m_rotVel(rotVel),
!                      m_nbOscillation(nbOscillation)
  {
     // The velocity must not be o
***************
*** 87,90 ****
--- 89,96 ----
     m_slopeRot = (0-m_rotVel) / (1-m_rotVelSecurityMargin); 
     m_rotB = -1*m_slopeRot;
+    
+    // Initialization
+    m_lastPos = 0;
+    m_lastSign = 1;
  }
  
***************
*** 160,176 ****
        
        // Fill the belt with the avoiding distance
!       int min=0;
        for(unsigned int i = 0; i<beltSize; ++i)
        {
             m_ellipseBelt[i] = evalEllipticalAvoidingThres(i*angularIncrement, distanceThres);
-            // Check if the minimal safety protection is respected
-            if(m_minEllipseLong != 0)
-            {
-               min = evalEllipticalAvoidingThres(i*angularIncrement, m_minEllipseLong);
-               if(m_ellipseBelt[i] < min)
-               {
-                  m_ellipseBelt[i] = min;
-               }
-            }
        }
        return true;
--- 166,173 ----
        
        // Fill the belt with the avoiding distance
!       //int min=0;
        for(unsigned int i = 0; i<beltSize; ++i)
        {
             m_ellipseBelt[i] = evalEllipticalAvoidingThres(i*angularIncrement, distanceThres);
        }
        return true;
***************
*** 183,214 ****
  }
  
- /*bool AvoidUtil::createEllipticalDirectionChangeThresBelt(unsigned int beltSize)
- {
-    if((beltSize == 0) or (beltSize > 360))
-    {
-       // Beltsize must be between 1 and 360 degree and distanceThres <> 0
-       return false;
-    }
-    else{
-       // Determine the angular increment for the belt
-       unsigned int angularIncrement = 360 / beltSize;
-       
-       // Resize the std::Vector
-       m_directionChangeBelt.resize(beltSize);
-       
-       // Fill the belt with the avoiding distance
-       for(unsigned int i = 0; i < beltSize; ++i)
-       {
-            m_directionChangeBelt[i] = evalEllipticalDirectionChangeThres(i*angularIncrement);
-       }
-       return true;
-    }
- }
- 
- const std::vector<unsigned int>& AvoidUtil::getEllipticalDirectionChangeThresBelt()
- {
-    return m_directionChangeBelt;
- }*/
- 
  bool AvoidUtil::createProximityMarginBelt(std::vector<int> rangeBelt)
  {
--- 180,183 ----
***************
*** 315,318 ****
--- 284,314 ----
        }
     }
+    
+    // Override the sign computation to keep the last value in case of frontal oscillation
+    if((position == 3) || (position == 9))
+    {
+       if(m_lastPos == position)
+       {
+          if(m_CurrentOscillation > 0)
+          {
+             sign = m_lastSign;
+          }
+          else
+          {
+             m_lastPos = 0;
+          }
+          --m_CurrentOscillation;
+       }
+       else
+       {
+          m_CurrentOscillation = m_nbOscillation;
+          m_lastPos = position;
+          m_lastSign = sign;
+       }
+    }
+    else
+    {
+       m_lastPos = position;
+    }
     return sign;
  }
***************
*** 393,408 ****
     return false;
  }
- /*
- bool AvoidUtil::isDirectionChangeNeeded(unsigned int position, unsigned int rangeValue)
- {
-    if(position >= m_directionChangeBelt.size())
-    {
-       throw AvoidUtilBeltPositionOutOfRange();
-    }
-    else
-    {
-       return (rangeValue < m_directionChangeBelt[position]);
-    }
- }*/
  
  int AvoidUtil::evalAvoidingLinVel(int velocity, double margin)
--- 389,392 ----
***************
*** 434,437 ****
--- 418,425 ----
        // Evaluate linear approximation variable
        vel = static_cast<int>(m_velSlope * margin + m_velB);
+       if(velocity<0)
+       {
+          vel *= -1;
+       }
     }
     return vel;