How can I make a foot-bot follow a heading vector expressed in the global frame of reference?
To clarify, I have a foot-bot that I want to move to a certain location. I know the global position of the foot-bot and the goal location so I create a vector between the two. However, this vector is expressed with respect to the global coordinate system. Since I know the orientation of the foot-bot, I use that to adjust it. I've written some code (modified from the flocking example), but when I run it on multiple robots, they each move in a different direction.
Code: Select all
SetWheelSpeedsFromVector(VectorToGoal());
CVector2 CInteraction::VectorToGoal() {
/* Calculate a normalized vector that points to the goal */
Real xCartesian = goalLocation.GetX()-GetPosition().GetX();
Real yCartesian = goalLocation.GetY()-GetPosition().GetY();
Real distanceToGoal = pow( pow(xCartesian,2.0)+pow(yCartesian,2.0) , 0.5);
Real angleToGoal = atan(yCartesian/xCartesian);
CVector2 goalVector = CVector2(distanceToGoal,angleToGoal);
Real orientationChange = -GetOrientation().GetValue();
CVector2 frameAdjustmentVector = CVector2(0,orientationChange);
goalVector += frameAdjustmentVector;
if(goalVector.Length() > 0.0f) {
/* Make the vector long as 1/4 of the max speed */
goalVector.Normalize();
goalVector*= 0.25f * m_sWheelTurningParams.MaxSpeed;
}
return goalVector;
}
Cheers,
Mostafa