bool Robot::returnToOrigin(){
long now = millis();
if (now - lastUpdateTime > UPDATE_PERIOD){
lastUpdateTime = now;
if (navigator.run()){
if (path.empty()){
return true;
}
else {
popWaypoint();
}
}
}
return false;
}
///Updates waypoints based on this unit's position
bool Unit::updateWaypoints()
{
if(!hasWaypoints())
return false;
Vector2<int> nextPoint = getCurrentWaypoint();
//If we are on the next waypoint, cycle waypoints
if(nextPoint.x == (int)floor(coord.x) && nextPoint.y == (int)floor(coord.y))
{
popWaypoint();
if(stance.compare(STANCE_PATROL) == 0 && nextcoord.size() > 1)
pushWaypoint(nextPoint);
return true;
}
return false;
}
