[Robotflow-cvs] RobotFlow/Behaviors/src AvoidPat.cc,1.3,1.4 SafeVelocityPat.cc,1.4,1.5

[MagellanPro <mag...@us...>]

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

Modified Files:
	AvoidPat.cc SafeVelocityPat.cc 
Log Message:
- Modification of the way rotation and velocity are calculated
- 


Index: SafeVelocityPat.cc
===================================================================
RCS file: /cvsroot/robotflow/RobotFlow/Behaviors/src/SafeVelocityPat.cc,v
retrieving revision 1.4
retrieving revision 1.5
diff -C2 -d -r1.4 -r1.5
*** SafeVelocityPat.cc	3 Jul 2005 17:39:21 -0000	1.4
--- SafeVelocityPat.cc	3 Jul 2005 23:54:19 -0000	1.5
***************
*** 149,153 ****
           if(abs(velIn) < m_min_vel)
               securityDist = m_min_distance;
!          else securityDist = m_slope*velIn + m_b;
           
           // Evaluate the security margin around the robot
--- 149,153 ----
           if(abs(velIn) < m_min_vel)
               securityDist = m_min_distance;
!          else securityDist = m_slope*abs(velIn) + m_b;
           
           // Evaluate the security margin around the robot

Index: AvoidPat.cc
===================================================================
RCS file: /cvsroot/robotflow/RobotFlow/Behaviors/src/AvoidPat.cc,v
retrieving revision 1.3
retrieving revision 1.4
diff -C2 -d -r1.3 -r1.4
*** AvoidPat.cc	3 Jul 2005 17:39:21 -0000	1.3
--- AvoidPat.cc	3 Jul 2005 23:54:19 -0000	1.4
***************
*** 140,145 ****
    
    // Slope of the line used to determine security distance
!   float m_slope;
!   float m_b;
  
  public:
--- 140,148 ----
    
    // Slope of the line used to determine security distance
!   float m_dist_slope;
!   float m_dist_b;
! 
!   float m_vel_slope;
!   float m_vel_b;
  
  public:
***************
*** 169,174 ****
       m_max_backward_vel = dereference_cast<int> (parameters.get("MAX_BACKWARD_VELOCITY"));
       
!      m_slope = (1.0*m_max_distance - m_min_distance) / m_max_robot_vel;
!      m_b = m_max_distance - m_slope * m_max_robot_vel; 
     }
  
--- 172,180 ----
       m_max_backward_vel = dereference_cast<int> (parameters.get("MAX_BACKWARD_VELOCITY"));
       
!      m_dist_slope = (1.0*m_max_distance - m_min_distance) / m_max_robot_vel;
!      m_dist_b = m_max_distance - m_dist_slope * m_max_robot_vel; 
! 
!      m_vel_slope = (m_max_avoid_vel-m_min_avoid_vel) / (4.0); 
!      m_vel_b = m_min_avoid_vel;	     
     }
  
***************
*** 189,193 ****
          if(abs(velIn) < m_min_avoid_vel)
              securityDist = m_min_distance;
!         else securityDist = m_slope*velIn + m_b;
          
          // Evaluate the security margin around the robot
--- 195,199 ----
          if(abs(velIn) < m_min_avoid_vel)
              securityDist = m_min_distance;
!         else securityDist = m_dist_slope*abs(velIn) + m_dist_b;
          
          // Evaluate the security margin around the robot
***************
*** 221,226 ****
          if(min_margin < 1){
              // Determine linear speed according to the security margin
!             int velOut = static_cast<int>(m_max_avoid_vel * min_margin);
!             if(velOut < m_min_avoid_vel) velOut = m_min_avoid_vel;
              
              // Determine rotational speed
--- 227,232 ----
          if(min_margin < 1){
              // Determine linear speed according to the security margin
!             //int velOut = static_cast<int>(m_max_avoid_vel * min_margin);
!             //if(velOut < m_min_avoid_vel) velOut = m_min_avoid_vel;
              
              // Determine rotational speed
***************
*** 228,232 ****
              
              // Using different relation depending of the closest obstacle to the robot
!             float slope;
              float b;
              switch (ind_min_margin){
--- 234,238 ----
              
              // Using different relation depending of the closest obstacle to the robot
!             /*float slope;
              float b;
              switch (ind_min_margin){
***************
*** 305,309 ****
                      if(ind_min_margin==8) rotation *= -1;
                      break;
!             }
              
              // Define FD output
--- 311,333 ----
                      if(ind_min_margin==8) rotation *= -1;
                      break;
!             }*/
! 
! 	    // Rotation version 2
! 	    switch(ind_min_margin){
! 		case 0:  rotation = m_max_rot; break;
! 		case 1:  rotation = static_cast<int>(m_max_rot * 0.75); break;
! 		case 2:  rotation = static_cast<int>(m_max_rot * 0.50); break;
! 		case 3:  rotation = static_cast<int>(m_max_rot * 0.25); break;
! 	        case 4:  rotation = static_cast<int>(m_max_rot * 0.10); break;
! 		case 8:  rotation = -1*static_cast<int>(m_max_rot * 0.10); break;
! 	        case 9:  rotation = -1*static_cast<int>(m_max_rot * 0.25); break;
! 	        case 10: rotation = -1*static_cast<int>(m_max_rot * 0.50); break;
! 	        case 11: rotation = -1*static_cast<int>(m_max_rot * 0.75); break;
! 	    }
! 
! 	    // Speed version 2
! 	    int indice = ind_min_margin;
! 	    if(indice > 4) indice = 12 - indice;
! 	    int velOut = static_cast<int>(m_vel_slope * indice + m_vel_b);
              
              // Define FD output
***************
*** 325,329 ****
              std::cout << "min_margin -> " << min_margin << std::endl;
              std::cout << "ind_min_margin -> " << ind_min_margin << std::endl;
!             std::cout << "slope -> " << slope << std::endl;
              std::cout << "rotation -> " << rotation << std::endl;
              std::cout << "velocity -> " << velOut << std::endl; 
--- 349,353 ----
              std::cout << "min_margin -> " << min_margin << std::endl;
              std::cout << "ind_min_margin -> " << ind_min_margin << std::endl;
! //            std::cout << "slope -> " << slope << std::endl;
              std::cout << "rotation -> " << rotation << std::endl;
              std::cout << "velocity -> " << velOut << std::endl;