[Robotflow-cvs] RobotFlow/Behaviors/src AvoidPat.cc,1.1,1.2 SafeVelocityPat.cc,1.2,1.3

SourceForge Headquarters 225 Broadway Suite 1600 San Diego, CA 92101 +1 (858) 422-6466

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

Modified Files:
	AvoidPat.cc SafeVelocityPat.cc 
Log Message:
- Ajusting speed control in both behavior (extra parameters)


Index: SafeVelocityPat.cc
===================================================================
RCS file: /cvsroot/robotflow/RobotFlow/Behaviors/src/SafeVelocityPat.cc,v
retrieving revision 1.2
retrieving revision 1.3
diff -C2 -d -r1.2 -r1.3
*** SafeVelocityPat.cc	3 Jul 2005 04:35:47 -0000	1.2
--- SafeVelocityPat.cc	3 Jul 2005 07:24:17 -0000	1.3
***************
*** 43,47 ****
   * @input_description Behavior activation.
   *
!  * @input_name VELOCITY_IN
   * @input_type int
   * @input_description Desired velocity of the robot
--- 43,47 ----
   * @input_description Behavior activation.
   *
!  * @input_name DESIRED_VELOCITY
   * @input_type int
   * @input_description Desired velocity of the robot
***************
*** 51,59 ****
   * @input_description Desired rotation of the robot
   *
!  * @input_name DIST_OBSTACLE
   * @input_type Vector<int>
   * @input_description Distance of the obstacle to the robot
   *
!  * @output_name VELOCITY_OUT
   * @output_type int
   * @output_description Output velocity
--- 51,59 ----
   * @input_description Desired rotation of the robot
   *
!  * @input_name RANGE_BELT
   * @input_type Vector<int>
   * @input_description Distance of the obstacle to the robot
   *
!  * @output_name VELOCITY
   * @output_type int
   * @output_description Output velocity
***************
*** 64,78 ****
   * @parameter_name MAX_VELOCITY
   * @parameter_type int
!  * @parameter_value 500
   * @parameter_description Maximum velocity allowed (mm/s).
   *
   * @parameter_name MAX_DISTANCE
   * @parameter_type int
!  * @parameter_value 1000
   * @parameter_description Maximum distance of the protection (mm)
   *
   * @parameter_name MIN_DISTANCE
   * @parameter_type int
!  * @parameter_value 100
   * @parameter_description Maximum distance of the protection (mm)
   *
--- 64,83 ----
   * @parameter_name MAX_VELOCITY
   * @parameter_type int
!  * @parameter_value 300
   * @parameter_description Maximum velocity allowed (mm/s).
   *
+  * @parameter_name MIN_VELOCITY
+  * @parameter_type int
+  * @parameter_value 75
+  * @parameter_description Minimum velocity to beat friction (mm/s).
+  *
   * @parameter_name MAX_DISTANCE
   * @parameter_type int
!  * @parameter_value 800
   * @parameter_description Maximum distance of the protection (mm)
   *
   * @parameter_name MIN_DISTANCE
   * @parameter_type int
!  * @parameter_value 150
   * @parameter_description Maximum distance of the protection (mm)
   *
***************
*** 83,100 ****
     
    //inputs
!   int velocityInID;
    int rotationID;
    int rangeID;
  
    //outputs
!   int velocityOutID;
  
    //parameters
    int m_max_vel;
    int m_max_distance;
    int m_min_distance;
    
    // Slope of the line used to determine security distance
!   float m;
  
  public:
--- 88,107 ----
     
    //inputs
!   int desiredVelID;
    int rotationID;
    int rangeID;
  
    //outputs
!   int velocityID;
  
    //parameters
    int m_max_vel;
+   int m_min_vel;
    int m_max_distance;
    int m_min_distance;
    
    // Slope of the line used to determine security distance
!   float m_slope;
!   float m_b;
  
  public:
***************
*** 104,124 ****
  
       //inputs
!      velocityInID = addInput("VELOCITY_IN");
       rotationID = addInput("ROTATION");
!      rangeID = addInput("DIST_OBSTACLE");
  
       //outputs
!      velocityOutID = addOutput("VELOCITY_OUT");
  
       // Parameters
       m_max_vel = dereference_cast<int> (parameters.get("MAX_VELOCITY"));
       m_max_distance = dereference_cast<int> (parameters.get("MAX_DISTANCE"));
       m_min_distance = dereference_cast<int> (parameters.get("MIN_DISTANCE"));
       
!      m = (1.0*m_max_distance - m_min_distance) /m_max_vel; 
     }
  
     virtual void calculate_behavior(int output_id, int count, Buffer &out) {
!      ObjectRef velInRef = getInput(velocityInID,count);
       ObjectRef rotationRef = getInput(rotationID,count);
       ObjectRef rangeValueRef = getInput(rangeID,count);
--- 111,133 ----
  
       //inputs
!      desiredVelID = addInput("DESIRED_VELOCITY");
       rotationID = addInput("ROTATION");
!      rangeID = addInput("RANGE_BELT");
  
       //outputs
!      velocityID = addOutput("VELOCITY");
  
       // Parameters
       m_max_vel = dereference_cast<int> (parameters.get("MAX_VELOCITY"));
+      m_min_vel = dereference_cast<int> (parameters.get("MIN_VELOCITY"));
       m_max_distance = dereference_cast<int> (parameters.get("MAX_DISTANCE"));
       m_min_distance = dereference_cast<int> (parameters.get("MIN_DISTANCE"));
       
!      m_slope = (1.0*m_max_distance - m_min_distance) / (m_max_vel - m_min_vel); 
!      m_b = m_max_distance - m_slope * m_max_vel;
     }
  
     virtual void calculate_behavior(int output_id, int count, Buffer &out) {
!      ObjectRef velInRef = getInput(desiredVelID,count);
       ObjectRef rotationRef = getInput(rotationID,count);
       ObjectRef rangeValueRef = getInput(rangeID,count);
***************
*** 130,134 ****
           // Determine the security distance
           int velIn = dereference_cast<int>(velInRef);
!          float securityDist = m*velIn + m_min_distance;
           
           // Evaluate the security margin around the robot
--- 139,146 ----
           // Determine the security distance
           int velIn = dereference_cast<int>(velInRef);
!          float securityDist = 0;
!          if(velIn < m_min_vel)
!              securityDist = m_min_distance;
!          else securityDist = m_slope*velIn + m_b;
           
           // Evaluate the security margin around the robot
***************
*** 187,190 ****
--- 199,205 ----
                velOut = static_cast<int>(velIn * marginFront);
           else velOut = static_cast<int>(velIn * marginBack);
+ 
+          if((velOut < m_min_vel) && (velIn > m_min_vel))
+              velOut = m_min_vel;
           
           // Debugging purpose only
***************
*** 207,211 ****
       }
       
!      (*outputs[velocityOutID].buffer)[count] = ObjectRef(Int::alloc(velOut));
     }//calculate_behavior
  
--- 222,226 ----
       }
       
!      (*outputs[velocityID].buffer)[count] = ObjectRef(Int::alloc(velOut));
     }//calculate_behavior
  

Index: AvoidPat.cc
===================================================================
RCS file: /cvsroot/robotflow/RobotFlow/Behaviors/src/AvoidPat.cc,v
retrieving revision 1.1
retrieving revision 1.2
diff -C2 -d -r1.1 -r1.2
*** AvoidPat.cc	3 Jul 2005 04:35:46 -0000	1.1
--- AvoidPat.cc	3 Jul 2005 07:24:17 -0000	1.2
***************
*** 59,70 ****
   * @output_description Behavior exploitation.
   *
!  * @parameter_name MAX_VELOCITY
   * @parameter_type int
!  * @parameter_value 300
   * @parameter_description Maximum velocity allowed (mm/s).
   *
   * @parameter_name MAX_ROTATION
   * @parameter_type int
!  * @parameter_value 25
   * @parameter_description Maximum rotation allowed (degree/s).
   *
--- 59,75 ----
   * @output_description Behavior exploitation.
   *
!  * @parameter_name MAX_AVOID_VELOCITY
   * @parameter_type int
!  * @parameter_value 200
   * @parameter_description Maximum velocity allowed (mm/s).
   *
+  * @parameter_name MIN_AVOID_VELOCITY
+  * @parameter_type int
+  * @parameter_value 75
+  * @parameter_description Minimum velocity to help the rotation (mm/s).
+  *
   * @parameter_name MAX_ROTATION
   * @parameter_type int
!  * @parameter_value 15
   * @parameter_description Maximum rotation allowed (degree/s).
   *
***************
*** 72,76 ****
   * @parameter_type int
   * @parameter_value 5
!  * @parameter_description Maximum rotation allowed (degree/s).
   *
   * @parameter_name MIN_ROTATION_GAIN
--- 77,81 ----
   * @parameter_type int
   * @parameter_value 5
!  * @parameter_description Minimum rotation to get something going (degree/s).
   *
   * @parameter_name MIN_ROTATION_GAIN
***************
*** 78,95 ****
   * @parameter_value 1.5
   * @parameter_description Gain to apply to the MIN_ROTATION value
!   *
   * @parameter_name MAX_DISTANCE
   * @parameter_type int
!  * @parameter_value 1000
   * @parameter_description Maximum distance of the protection (mm)
   *
   * @parameter_name MIN_DISTANCE
   * @parameter_type int
!  * @parameter_value 300
!  * @parameter_description Maximum distance of the protection (mm)
   *
   * @parameter_name DIST_BACKWARD
   * @parameter_type int
!  * @parameter_value 300
   * @parameter_description Distance to go backward
   *
--- 83,105 ----
   * @parameter_value 1.5
   * @parameter_description Gain to apply to the MIN_ROTATION value
!  *
!  * @parameter_name MAX_ROBOT_VELOCITY
!  * @parameter_type int
!  * @parameter_value 300
!  * @parameter_description Maximum distance of the protection (mm)
!  *
   * @parameter_name MAX_DISTANCE
   * @parameter_type int
!  * @parameter_value 600
   * @parameter_description Maximum distance of the protection (mm)
   *
   * @parameter_name MIN_DISTANCE
   * @parameter_type int
!  * @parameter_value 150
!  * @parameter_description Minimum distance of the protection (mm)
   *
   * @parameter_name DIST_BACKWARD
   * @parameter_type int
!  * @parameter_value 200
   * @parameter_description Distance to go backward
   *
***************
*** 108,112 ****
  
    //parameters
!   int m_max_vel;
    int m_max_rot;
    int m_min_rot;
--- 118,124 ----
  
    //parameters
!   int m_max_avoid_vel;
!   int m_min_avoid_vel;
!   int m_max_robot_vel;
    int m_max_rot;
    int m_min_rot;
***************
*** 117,121 ****
    
    // Slope of the line used to determine security distance
!   float m_vel_m;
  
  public:
--- 129,134 ----
    
    // Slope of the line used to determine security distance
!   float m_slope;
!   float m_b;
  
  public:
***************
*** 133,137 ****
  
       // Parameters
!      m_max_vel = dereference_cast<int> (parameters.get("MAX_VELOCITY"));
       m_max_rot = dereference_cast<int> (parameters.get("MAX_ROTATION"));
       m_min_rot = dereference_cast<int> (parameters.get("MIN_ROTATION"));
--- 146,152 ----
  
       // Parameters
!      m_max_avoid_vel = dereference_cast<int> (parameters.get("MAX_AVOID_VELOCITY"));
!      m_min_avoid_vel = dereference_cast<int> (parameters.get("MIN_AVOID_VELOCITY"));
!      m_max_robot_vel = dereference_cast<int> (parameters.get("MAX_ROBOT_VELOCITY"));
       m_max_rot = dereference_cast<int> (parameters.get("MAX_ROTATION"));
       m_min_rot = dereference_cast<int> (parameters.get("MIN_ROTATION"));
***************
*** 141,145 ****
       m_dist_backward = dereference_cast<int> (parameters.get("DIST_BACKWARD"));
       
!      m_vel_m = (1.0*m_max_distance - m_min_distance) /m_max_vel; 
     }
  
--- 156,161 ----
       m_dist_backward = dereference_cast<int> (parameters.get("DIST_BACKWARD"));
       
!      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; 
     }
  
***************
*** 151,161 ****
           
          // Get the actual velocity of the robot
!         int velIn = m_max_vel;
!         /*if(!velInRef->isNil()){
             velIn = dereference_cast<int>(velInRef);
!         }*/
          
          // Determine the security distance
!         float securityDist = m_vel_m*velIn + m_min_distance;
          
          // Evaluate the security margin around the robot
--- 167,180 ----
           
          // Get the actual velocity of the robot
!         int velIn = m_max_robot_vel;
!         if(!velInRef->isNil()){
             velIn = dereference_cast<int>(velInRef);
!         }
          
          // Determine the security distance
!         float securityDist = 0;
!         if(velIn < m_min_avoid_vel)
!             securityDist = m_min_distance;
!         else securityDist = m_slope*velIn + m_b;
          
          // Evaluate the security margin around the robot
***************
*** 184,188 ****
          if(min_margin < 1){
              // Determine linear speed according to the security margin
!             int velOut = static_cast<int>(m_max_vel * min_margin);
              
              // Determine rotational speed
--- 203,208 ----
          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