[Robotflow-cvs] RobotFlow/Behaviors/src Goto.cc,1.3,1.4

[Carle C. <car...@us...>]

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

Modified Files:
	Goto.cc 
Log Message:
calculate diff angle correctly

Index: Goto.cc
===================================================================
RCS file: /cvsroot/robotflow/RobotFlow/Behaviors/src/Goto.cc,v
retrieving revision 1.3
retrieving revision 1.4
diff -C2 -d -r1.3 -r1.4
*** Goto.cc	11 Mar 2005 18:55:12 -0000	1.3
--- Goto.cc	20 Mar 2005 22:53:48 -0000	1.4
***************
*** 98,102 ****
   * @parameter_type int
   * @parameter_value 15
! 
  END*/
  
--- 98,107 ----
   * @parameter_type int
   * @parameter_value 15
!  *
!  * @parameter_name VELOCITY_ANGLE_RATIO
!  * @parameter_description  Max Velocity ratio to apply when velocity angle is activated (float)
!  * @parameter_type float
!  * @parameter_value 0.4
!  *
  END*/
  
***************
*** 124,127 ****
--- 129,133 ----
    int m_heading_tol;
    int m_activate_velocity_angle;
+   float m_activate_velocity_ratio;
    
  public:
***************
*** 148,151 ****
--- 154,158 ----
       m_heading_tol = dereference_cast<int> (parameters.get("HEADING_TOLERANCE"));
       m_activate_velocity_angle = dereference_cast<int> (parameters.get("ACTIVATE_VELOCITY_ANGLE"));
+      m_activate_velocity_ratio = dereference_cast<float> (parameters.get("VELOCITY_ANGLE_RATIO"));
  
     }
***************
*** 215,225 ****
            
            newHeading = ((180.0 * newHeading) / M_PI) + (double)(m_heading_offset);
            if(newHeading < 0)
               newHeading = 360.0 + newHeading;
            
!           double diffHeading = heading - newHeading;
            
            
!           std::cout << "newHeading: " << newHeading << std::endl;
            std::cout << "diffHeading: " << diffHeading << std::endl;
            
--- 222,253 ----
            
            newHeading = ((180.0 * newHeading) / M_PI) + (double)(m_heading_offset);
+           
+           std::cout << "newHeading: " << newHeading << std::endl;
            if(newHeading < 0)
               newHeading = 360.0 + newHeading;
            
!           if(heading > 270 && newHeading < 90)
!           {
!              heading -= 90;
!              newHeading = 270 + newHeading;
!           }
!           else if(heading < 90 && newHeading > 270)
!           {
!             heading = 270 + heading;
!             newHeading -= 90;
!           }
            
            
!           double diffHeading = heading - newHeading;
!           
!           if(diffHeading > 180)
!           {
!              diffHeading = 360 - diffHeading;
!           }
!           else if(diffHeading < -180)
!           {
!              diffHeading = -360 -diffHeading;
!           }
!                     
            std::cout << "diffHeading: " << diffHeading << std::endl;
            
***************
*** 231,246 ****
            else
            {
!              if((diffHeading > 0 && diffHeading < 180) || diffHeading < -180)
               {
                  (*outputs[rotationID].buffer)[count] = ObjectRef(Int::alloc(-m_max_rotation));
               }
!              else
               {
                  (*outputs[rotationID].buffer)[count] = ObjectRef(Int::alloc(m_max_rotation));
               }
               
               if(abs(diffHeading) < m_activate_velocity_angle)
               {
!                 (*outputs[velocityID].buffer)[count] = ObjectRef(Int::alloc(m_max_velocity/5));
               }
               else
--- 259,278 ----
            else
            {
!              if(diffHeading > 0 && diffHeading < 180)
               {
                  (*outputs[rotationID].buffer)[count] = ObjectRef(Int::alloc(-m_max_rotation));
               }
!              else if(diffHeading < 0 && diffHeading > -180)
               {
                  (*outputs[rotationID].buffer)[count] = ObjectRef(Int::alloc(m_max_rotation));
               }
+              else
+              {
+                 (*outputs[rotationID].buffer)[count] = ObjectRef(Int::alloc(0));
+              }
               
               if(abs(diffHeading) < m_activate_velocity_angle)
               {
!                 (*outputs[velocityID].buffer)[count] = ObjectRef(Int::alloc(int(m_max_velocity * m_activate_velocity_ratio)));
               }
               else