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
|
