void DefendFarFromBall::perform(Robot *robot)
{
GameModel *gm = GameModel::getModel();
Point robPoint = robot->getRobotPosition();
Point ballPoint = gm->getBallPoint();
Point myGoal = gm->getMyGoal();
double direction = Measurments::angleBetween(myGoal, ballPoint);
Point defensiveWall(cos(direction)*DISTANCE + myGoal.x,
sin(direction)*DISTANCE + myGoal.y);
/* Check if there are any opp robots within 3000 distance of the ball
* This boolean is used to determine if the goalie should wait
* before kicking the ball to a teammate in case an opp robot intersepts
* the ball
*/
bool safeToKick = 1;
for(Robot* opRob:gm->getOponentTeam())
{
if (Measurments::distance(opRob->getRobotPosition(),ballPoint) < 3000)
safeToKick = 0;
}
bool isScoreHazard =
Measurments::distance(myGoal, ballPoint) < 1200
and not(Measurments::isClose(robPoint, ballPoint, 100))
and lastKickCounter <= 0
and safeToKick;
if(isScoreHazard or isKickingAwayBall) {
if(wasNotPreviousScoreHazard) {
KTPSkill = new Skill::KickToPoint(Point(0,0), 50*M_PI/180);
isKickingAwayBall = true;
wasNotPreviousScoreHazard = false;
}
if(KTPSkill->perform(robot) or
Measurments::distance(ballPoint, myGoal) > 1200){
lastKickCounter = 100;
wasNotPreviousScoreHazard = true;
isKickingAwayBall = false;
delete KTPSkill;
KTPSkill = nullptr;
}
} else {
if(lastKickCounter > 0)
--lastKickCounter;
Point defensiveWall(cos(direction)*DISTANCE + myGoal.x,
sin(direction)*DISTANCE + myGoal.y);
setMovementTargets(defensiveWall, direction, false, false);
GenericMovementBehavior::perform(robot, Movement::Type::facePoint);
}
}
/* This is the function that communicates outside the FPPA namespace to define
* obstacle information. In our case, that is getting the GameModel and all the robots
* from that. However, I would like to have this more generalized in the future.
*/
void buildObstacleCollection()
{
GameModel* mod = GameModel::getModel();
const auto& myTeam = mod->getMyTeam();
const auto& opTeam = mod->getOponentTeam();
currentFrameObstacles.clear();
currentFrameObstacles.reserve(myTeam.size() + opTeam.size() + 1);
for(Robot* rob : myTeam)
currentFrameObstacles.push_back(rob->getRobotPosition());
for(Robot* rob : opTeam)
currentFrameObstacles.push_back(rob->getRobotPosition());
currentFrameObstacles.push_back(mod->getBallPoint());
#if FPPA_DEBUG
// std::cout << "[FPPA] All Obstacles: " << std::endl;
//for(Point pt : *obstacles) std::cout << pt.toString() << std::endl;
#endif
}
void FreeKickStrategy::assignBeh()
{
GameModel *gm = GameModel::getModel();
vector <Robot*> myTeam;
myTeam = gm->getMyTeam();
if ((gm->getGameState() == 'F' && TEAM == TEAM_BLUE)
|| (gm->getGameState() == 'f' && TEAM == TEAM_YELLOW))
{
BehaviorAssignment<KickToGoal> kickToGoalAssignment;
kickToGoalAssignment.setSingleAssignment(true);
BehaviorAssignment<SimpleBehaviors> simpleAssignment;
simpleAssignment.setSingleAssignment(true);
BehaviorAssignment<DefendFarFromBall> golieAssignment;
golieAssignment.setSingleAssignment(true);
for (Robot* rob: myTeam)
{
if (rob->getID() == 5)
golieAssignment.assignBeh(rob);
}
Robot *closestRobot;
int closestRobotID;
Point ballPoint = gm->getBallPoint();
/*Finds the closest robot to the ball point (assuming the
* ball is where the robots should perform free kick)
* and its ID
* If there is only one robot on the field, that one robot
* will perform the free kick
* */
if (myTeam.size() == 1)
closestRobot = myTeam.at(0);
else if (myTeam.size() > 1)
{
if (myTeam.at(0)->getID() != 5)
closestRobot = myTeam.at(0);
else
closestRobot = myTeam.at(1);
for (unsigned i = 1; i < myTeam.size(); i++)
{
if (myTeam.at(i)->getID() != 5)
{
Point iPos = myTeam.at(i)->getRobotPosition();
Point closestPos = closestRobot->getRobotPosition();
if (Measurments::distance(iPos, ballPoint) < Measurments::distance(closestPos, ballPoint))
closestRobot = myTeam.at(i);
}
}
closestRobotID = closestRobot->getID();
}
kickToGoalAssignment.assignBeh(closestRobot); //lets the closest robot to the ball to perform the free kick
if (myTeam.size() > 1) // assigns simple behavior to the rest of robots
{
for (unsigned i = 0; i < myTeam.size(); i++)
{
if (myTeam.at(i)->getID() != closestRobotID && myTeam.at(i)->getID() != 5)
simpleAssignment.assignBeh(myTeam.at(i));
}
}
}
else if ((gm->getGameState() == 'f' && TEAM == TEAM_BLUE)
|| (gm->getGameState() == 'F' && TEAM == TEAM_YELLOW))
{
BehaviorAssignment<DefendFarFromBall> golieAssignment;
golieAssignment.setSingleAssignment(true);
for (Robot* rob: myTeam)
{
if (rob->getID() == 5)
golieAssignment.assignBeh(rob);
}
BehaviorAssignment<SimpleBehaviors> simpleAssignment;
simpleAssignment.setSingleAssignment(true);
for (unsigned i = 0; i < myTeam.size(); i++)
{
if (myTeam.at(i)->getID() != 5)
simpleAssignment.assignBeh(myTeam.at(i));
}
}
}
void KickToGoal::perform(Robot * r)
{
GameModel* gm = GameModel::getModel();
Point ball = gm->getBallPoint();
Point goal = gm->getOpponentGoal();
Point rob = r->getRobotPosition();
float robAng = r->getOrientation();
float goalToBall = Measurments::angleBetween(goal,ball);
float ballToGoal = Measurments::angleBetween(ball, goal);
Point behindBall(BEHIND_RADIUS*cos(goalToBall)+ball.x,
BEHIND_RADIUS*sin(goalToBall)+ball.y);
// Create a different skill depending on the state
switch (state) {
case goingBehind:
target = behindBall;
setMovementTargets(behindBall, ballToGoal, true);
GenericMovementBehavior::perform(r, Movement::Type::Default);
break;
case approaching:
setMovementTargets(ball, ballToGoal, false);
GenericMovementBehavior::perform(r, Movement::Type::SharpTurns);
break;
case kicking:
{
Skill::Kick k(0, 0);
k.perform(r);
}
break;
case stopping:
{
Skill::Stop stop;
stop.perform(r);
}
break;
}
// Evaluate possible transitions
switch (state){
case goingBehind:
cout << "going behind" << endl;
// cout << "1\t" << Measurments::distance(behindBall, rob) << endl;
// cout << "2\t" << abs(Measurments::angleDiff(robAng, ballToGoal))/M_PI*180 << endl;
if (Measurments::distance(behindBall, rob) < CLOSE_ENOUGH &&
Measurments::isClose(robAng, ballToGoal, ANGLE)) {
state = approaching;
target = ball;
}
else if (!Measurments::isClose(target, behindBall, CLOSE_ENOUGH))
{
state = goingBehind;
}
break;
case approaching:
cout << "approching" << endl;
// cout << "1\t" << Measurments::distance(ball, rob) << endl;
// cout << "2\t" << abs(Measurments::angleDiff(robAng, ballToGoal))/M_PI*180 << endl;
if (Measurments::distance(ball, rob) < CLOSE_ENOUGH &&
Measurments::isClose(robAng, ballToGoal, ANGLE))
state = kicking;
else if (!Measurments::isClose(target, ball, CLOSE_ENOUGH))
{
state = goingBehind;
}
// else if (Measurments::distance(ball, rob) > CLOSE_ENOUGH*2) {
// state = goingBehind;
// }
break;
case kicking:
cout << "kicking" << endl;
state = stopping;
break;
case stopping:
cout << "stopping" << endl;
break;
}
}