void execute(const Param& tParam)
{
printf("DragtoGoal BotID: %d\n",botID);
static int stt = 0;
// Select the skill to be executed next
Vector2D<int> fastBallPos;
Vector2D<float> origin(0, 0);
fastBallPos = Vector2D<int>((int)state->ballPos.x + 0.3 * state->ballVel.x, (int)state->ballPos.y + 0.3 * state->ballVel.y);
if(state->ballVel.absSq() > MOVING_BALL_VELOCITY * MOVING_BALL_VELOCITY)
{
Util::Logger::toStdOut("Capturing fast Ball %f\n", state->ballVel.absSq());
float captureDist = Vector2D<int>::dist(state->ballPos, state->homePos[botID]);
// float captureAngle = fabs(firaNormalizeAngle(Vector2D<float>::angle(state->ballVel, origin)-Vector2D<int>::angle(state->homePos[botID], state->ballPos)));
// if(captureAngle < PI/3) {
// Util::Logger::toStdOut("Ball now in range\n");
// fastBallPos = Vector2D<int>((int)state->ballPos.x + 0.1*state->ballVel.x, (int)state->ballPos.y + 0.1*state->ballVel.y);
// }
}
Vector2D<int> goalPoint(HALF_FIELD_MAXX, 0);
float theta = normalizeAngle(Vector2D<int>::angle(goalPoint, fastBallPos));
float angleWithBall = normalizeAngle(Vector2D<int>::angle(fastBallPos, state->homePos[botID]));
//Vector2D<int> finalPoint = state->homePos[botID] + Vector2D<int>(2*BOT_BALL_THRESH*cos(theta), 2*BOT_BALL_THRESH*sin(theta));
Vector2D<int> targetPoint = fastBallPos - Vector2D<int>(2 * BOT_BALL_THRESH * cos(theta), 2 * BOT_BALL_THRESH * sin(theta));
float dist = Vector2D<int>::dist(targetPoint, state->homePos[botID]);
//printf("Drag to Goal: %f, %d, %f, \n", dist, 2*BOT_BALL_THRESH, fabs(firaNormalizeAngle(theta)));
float sign = (state->homePos[botID].x - fastBallPos.x) * (state->homePos[botID].x - HALF_FIELD_MAXX);
printf("state: %d\n", stt);
if(stt == 0 && isBallInMyWideAngleRange() && sign > 0)
{
stt = 1;
}
if(stt == 1 && (!isBallInMyLastHopeRange(botID) || sign < 0))
{
stt = 0;
}
if(stt == 1)
{
Util::Logger::toStdOut("Dribbling to goal %d %d", goalPoint.x, goalPoint.y);
gotoPointExact(state->ballPos.x, state->ballPos.y, true, normalizeAngle(Vector2D<int>::angle( goalPoint, fastBallPos )), MAX_BOT_SPEED);
}
else
{
//Util::Logger::toStdOut("Going to ball");
gotoPointExact(targetPoint.x, targetPoint.y, true, Vector2D<int>::angle(goalPoint, state->homePos[botID]), 0.7*MAX_BOT_SPEED);
}
if (state->pr_oppBall || state->pr_looseBall/*||state->pr_goalscored*/)
{
// tState = COMPLETED;
}
}
float CreatorPassSkill::calculateGoalieEffect()
{
/// How much we will like the creator to backoff
float moveBack = 0.0f;
/// Find the opponent which is closest to the goal point
Pair goalPoint(sp->field.THEIR_GOAL_LINE,(sp->field.LEFT_GOAL_POST+sp->field.RIGHT_GOAL_POST+2)/2);
float closest = 32000.0f;
RobotIndex closeRobot = NO_ROBOT;
Pair robot=getLocation(robotID,*currentVisionData,*sp);
for(RobotIndex i=ROBOT0;i<NUM_PLAYERS_ON_TEAM;i++){
Pair C(getLocation(sp->general.OTHER_TEAM,i,*currentVisionData));
if(dist(C,goalPoint) < closest)
{
closest = dist(C,goalPoint);
closeRobot = i;
}
}
/// If the distance of our robot from the closest robot
/// is greater than certain Threshold
if(closeRobot != NO_ROBOT)
{
Pair closeRobotLocation(getLocation(sp->general.OTHER_TEAM,closeRobot,*currentVisionData));
if(dist(robot,closeRobotLocation) < GOALIE_DISTANCE_THRESHOLD)
{
moveBack = GOALIE_DISTANCE_THRESHOLD - dist(robot,closeRobotLocation);
}
}
return moveBack;
}
