/**
 * @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);
}
Example #2
0
 /*****************************************************************************
 * 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;
 }
Example #3
0
 /*****************************************************************************
 * 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;
}
Example #5
0
 /*****************************************************************************
 * 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());
 }