/**
* @brief
*
* @param path
* @return bool
*/
bool NewRoboControl::drivePath(std::vector<Position>* path)
{
Position *target = NULL;
for (std::vector<Position>::iterator it = path->begin() ; it != path->end() ; it++)
{
target = &(*it);
if (!IsOnTarget(*target, false))
{
double d = 0;
Position prevPos = GetPos();
for (; it != path->end() ; it++)
{
Position pos = *it;
d += pos.DistanceTo(prevPos);
prevPos = pos;
}
return cruisetoBias(target->GetX(), target->GetY(), 700, -10, 30, d);
}
}
if (!target || IsOnTarget(*target))
{
RandomMove();
return true;
}
return cruisetoBias(target->GetX(), target->GetY(), 600);
}
/*****************************************************************************
* Function - Operator -
* DESCRIPTION:
*****************************************************************************/
Area Area::operator -(Position& Pos)
{
Area new_area;
new_area.Set(mUpperLeft.GetX() - Pos.GetX(),
mUpperLeft.GetY() - Pos.GetY(),
mLowerRight.GetX() - Pos.GetX(),
mLowerRight.GetY() - Pos.GetY() );
return new_area;
}
/*****************************************************************************
* Function - BringPositionWithinArea
* DESCRIPTION:
* Brings the Position within the area
*****************************************************************************/
void Position::BringPositionWithinArea(Area& Ar)
{
Position UpperLeftPos = Ar.GetUpperLeft();
Position LowerRightPos = Ar.GetLowerRight();
if (mX < UpperLeftPos.GetX())
{
mX = UpperLeftPos.GetX();
}
else if (mX > LowerRightPos.GetX())
{
mX = LowerRightPos.GetX();
}
if (mY < UpperLeftPos.GetY())
{
mY = UpperLeftPos.GetY();
}
else if (mY > LowerRightPos.GetY())
{
mY = LowerRightPos.GetY();
}
}
/**
* @brief this function check if the the robot reach the target
*
* @param current is the current position of the robot
* @param target is the target position of the robot
* @param precise defines how precise the robot should be around the target
* @return bool return ture if the robot reaches the target
*/
bool NewRoboControl::IsOnTarget(Position current, Position target, bool precise)
{
double margin = precise ? 0.05 : 0.2;
return fabs(current.GetX()-target.GetX()) < margin && fabs(current.GetY()-target.GetY()) < margin;
}
/*****************************************************************************
* Function - Operator -=
* DESCRIPTION:
*****************************************************************************/
void Area::operator -=(Position& Pos)
{
mUpperLeft.Set(mUpperLeft.GetX() - Pos.GetX(),mUpperLeft.GetY() - Pos.GetY());
mLowerRight.Set(mLowerRight.GetX() - Pos.GetX(),mLowerRight.GetY() - Pos.GetY());
}