[Robotflow-cvs] RobotFlow/Behaviors/src GotoPat.cc,1.1,1.2

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

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

Modified Files:
	GotoPat.cc 
Log Message:
1) Correction of the heading in GotoPat.cc


Index: GotoPat.cc
===================================================================
RCS file: /cvsroot/robotflow/RobotFlow/Behaviors/src/GotoPat.cc,v
retrieving revision 1.1
retrieving revision 1.2
diff -C2 -d -r1.1 -r1.2
*** GotoPat.cc	18 Jun 2005 15:08:20 -0000	1.1
--- GotoPat.cc	18 Jun 2005 16:32:17 -0000	1.2
***************
*** 108,113 ****
   * @parameter_value 3
   *
!  * @parameter_name MIN_DISTANCE_INTERMEDIATE_GOAL
!  * @parameter_description  Minimum distance to consider an intermediate goal (mm)
   * @parameter_type int
   * @parameter_value 1000
--- 108,114 ----
   * @parameter_value 3
   *
!  *
!  * @parameter_name DISCARD_DIST_INTER_GOAL
!  * @parameter_description  Distance to discard intermediate goal
   * @parameter_type int
   * @parameter_value 1000
***************
*** 143,147 ****
    std::vector<int> m_angle_chart;     // Not an actual parameter
    
!   // angularCorrection in degrees
    int speedValue(float angularCorrection)
    {
--- 144,148 ----
    std::vector<int> m_angle_chart;     // Not an actual parameter
    
!   // speed indice according to the angularCorrection to apply
    int speedValue(float angularCorrection)
    {
***************
*** 183,187 ****
       m_heading_offset = dereference_cast<int> (parameters.get("HEADING_OFFSET"));
       m_goal_tol = dereference_cast<int> (parameters.get("GOAL_TOLERANCE"));
!      m_discard_distance = dereference_cast<int> (parameters.get("MIN_DISTANCE_INTERMEDIATE_GOAL"));
       
       // Calculate speed lookup table
--- 184,188 ----
       m_heading_offset = dereference_cast<int> (parameters.get("HEADING_OFFSET"));
       m_goal_tol = dereference_cast<int> (parameters.get("GOAL_TOLERANCE"));
!      m_discard_distance = dereference_cast<int> (parameters.get("DISCARD_DIST_INTER_GOAL"));
       
       // Calculate speed lookup table
***************
*** 225,228 ****
--- 226,230 ----
         sstr >> y;
         sstr >> intermediate;
+        intermediate = !intermediate;
         
         double xpos = (double)(dereference_cast<int>(xPosValue));
***************
*** 230,236 ****
         double heading = (double)(dereference_cast<int>(headingValue));
         
-        //if(heading < 0)
-        //   heading = 360.0 + heading;
-        
         std::cout << "intermediate: " << intermediate << std::endl;       
         std::cout << "x: " << x << std::endl;
--- 232,235 ----
***************
*** 244,248 ****
         float distance = sqrt((x-xpos)*(x-xpos) + (y-ypos)*(y-ypos));
         
!        if (intermediate==false && distance < m_discard_distance){
             // Discard the goal because it's too close of the robot
             (*outputs[rotationID].buffer)[count] = ObjectRef(Int::alloc(0));
--- 243,247 ----
         float distance = sqrt((x-xpos)*(x-xpos) + (y-ypos)*(y-ypos));
         
!        if (intermediate && distance < m_discard_distance){
             // Discard the goal because it's too close of the robot
             (*outputs[rotationID].buffer)[count] = ObjectRef(Int::alloc(0));
***************
*** 256,259 ****
--- 255,267 ----
            double diffHeading = newHeading*57.2958 - heading;
            
+           std::cout << "newHeading: " << newHeading*57.2958 << std::endl;
+           std::cout << "diffHeading: " << diffHeading << std::endl;
+           
+           // Prevent over 180 degrees correction
+           if(fabs(diffHeading) > 180){
+               if(diffHeading<0) diffHeading += 360;
+               else diffHeading -= 360;
+           }
+           
            // Compute motor command
            int sign = (diffHeading < 0)?-1:1;
***************
*** 262,265 ****
--- 270,279 ----
            int rotation = sign * m_rotation_chart.at(speedInd);
            
+           // Decrease speed if near goal
+           if(!intermediate && distance<m_discard_distance) {
+               velocity /= 2;
+               rotation /= 2;
+           }
+           
            // Send motor command
            (*outputs[rotationID].buffer)[count] = ObjectRef(Int::alloc(rotation));
***************
*** 271,279 ****
            (*outputs[rotationID].buffer)[count] = ObjectRef(Int::alloc(0));
            (*outputs[velocityID].buffer)[count] = ObjectRef(Int::alloc(0));
            std::cout << "onGoal" << std::endl;
            std::cout << "rotation: " << (*outputs[rotationID].buffer)[count] << std::endl;
            std::cout << "velocity: " << (*outputs[velocityID].buffer)[count] << std::endl;
            std::cout << std::endl;
-           (*outputs[completedID].buffer)[count] = ObjectRef(Bool::alloc(true));
         }
         
--- 285,293 ----
            (*outputs[rotationID].buffer)[count] = ObjectRef(Int::alloc(0));
            (*outputs[velocityID].buffer)[count] = ObjectRef(Int::alloc(0));
+           (*outputs[completedID].buffer)[count] = ObjectRef(Bool::alloc(true));
            std::cout << "onGoal" << std::endl;
            std::cout << "rotation: " << (*outputs[rotationID].buffer)[count] << std::endl;
            std::cout << "velocity: " << (*outputs[velocityID].buffer)[count] << std::endl;
            std::cout << std::endl;
         }