attacker.cpp

FIRA 5V5比赛中一个机器人源代码 本科毕业设计做的

// Attacker.cpp: implementation of the CAttacker class.
//
//////////////////////////////////////////////////////////////////////

#include "stdafx.h"
#include "Attacker.h"
#include <math.h>
#include "Computation.h"

//////////////////////////////////////////////////////////////////////
// Construction/Destruction
//////////////////////////////////////////////////////////////////////


CAttacker::CAttacker(Environment *envPointer)  : CBasicAction(envPointer)
{

}

CAttacker::~CAttacker()
{
}

void CAttacker::wait(Environment *env,double lowerLimit,double higherLimit, int whichRobot)
{
	Vector3D dest;
	dest.y = env->predictedBall.pos.y;
	
	if(env->predictedBall.pos.x < lowerLimit)//如果球坐标小于最小极限
	{
		dest.x = lowerLimit;//则两者相等
	}
	else
	{
		if(env->predictedBall.pos.x > higherLimit)
		{
			dest.x = higherLimit;//同上
		}
		else
		{
			dest.x = env->predictedBall.pos.x;//否则，就以计算出的球坐标作为dest.x
		}
	}

	if(env->userData->shootArea == 1)//如果处在射门区域，就将dest这个3d变量的横坐标设置为球的下一运行位置，直接射门 。是说将whichRobot指带的机器人放到dest位置
	{
		dest.x = env->predictedBall.pos.x;
		shootStraight(&(env->home[whichRobot]));
	}

	position(dest,whichRobot);//将whichRobot指带的机器人放到dest位置
	return;
}

void CAttacker::lexAttacker(Environment *env, int whichRobot)//平常进攻方式
{

	Vector3D pointToGo;
	Vector3D oppoCentreGoal, predictedPos;
	double howFar;

	
	howFar = predictHowFar(env, whichRobot);//use lex predict ball or not??

	predictedPos = predictedBall(env, howFar);//预测球的的位置

	Vector3D upperGoalPos,lowerGoalPos;
	upperGoalPos.x = FRIGHTX;
	lowerGoalPos.x = FRIGHTX;
	upperGoalPos.y = 47.7151;
	lowerGoalPos.y = 35.5756;
	double AIM_RADIUS = 7;//DECIDE AIM THE BALL TO GOAL FROM HOW MUCH DISTANCE AWAY FROM BALL该在什么时候踢球
	double ORBIT_DISTANCE = 5;//DECIDE ROBOT SHOUD ORBIT WHAT DISTANCE FROM BALL WHEN ROBOT IS IN FRONT OF THE BALL机器人带球时应该与球保持的距离
	double gradient, angle;
	oppoCentreGoal.x = FRIGHTX;
	oppoCentreGoal.y = CENTERY;
//	pointToGo.x = env->currentBall.pos.x - 4;
	
	
	angle = findPointToPointAngle(env->currentBall.pos, oppoCentreGoal);//求射门角度
	pointToGo.x = env->currentBall.pos.x + cos(angle*PI/180) * 4;//lex:distance constant 4 behind ball and n oppo goal, better than direct put 4 x-asix behind the ball....
	
	//good formula.... i like it
	if(env->userData->homeGoal == 1)//如果球在本方手中
	{
		wait(env,50,FRIGHTX,whichRobot);//则机器人在横坐标为50到对方球门的位置之间移动
	}
	else if(env->userData->upper == 1)//lex:y >70如果球在对方手中，并且从y>70的边路进攻
	{
		if(findDistance(env->home[whichRobot].pos,env->currentBall.pos) < 3.2)//如果离球最近的机器人与球距离小于3.2
		{
			spin(&env->home[whichRobot], 125, 1);//左轮速度为125，右轮为1
		}

		else if(env->home[whichRobot].pos.x < env->currentBall.pos.x)//如果本方有机器人的位置和球的位置相同
		{
			sineTurn(posToGoal(env, whichRobot)/*predictedPos*/,whichRobot);//以曲线带球到射门位置
			
		}
		else//若本方无机器人与球接近
		{
			pointToGo.x = predictedPos.x - 4;//将机器人的横坐标向本方移动4
			pointToGo.y = predictedPos.y;
			sineTurn(pointToGo,whichRobot);//以pointToGo为目标做曲线运动
		}
	}
	else if(env->userData->lower == 1)//若球在对方手中并从下边进攻
	{
		
		if(findDistance(env->home[whichRobot].pos,env->currentBall.pos) < 3.2)//如果离球最近的机器人与球距离小于3.2
		{
			spin(&env->home[whichRobot], 125, 0);//左轮速度为125，右轮为0
		}

		else if(env->home[whichRobot].pos.x < predictedPos.x)//如果本方有机器人的横坐标小于预测的横坐标
		{

			sineTurn(posToGoal(env, whichRobot)/*predictedPos*/,whichRobot);//则应该一曲线跑到射门点
			
		}
		else//若本方无机器人与球接近
		{
			pointToGo.x = predictedPos.x - 4;//将机器人的横坐标向本方移动4
			pointToGo.y = predictedPos.y;
			sineTurn(pointToGo,whichRobot);//以pointToGo为目标做曲线运动
		}
	}
	else if((env->currentBall.pos.x >82) && (env->currentBall.pos.y > /*56.57*/64.54 || env->currentBall.pos.y < 18.17/*25.92*/))//lex:oppo goal outer line如果对方在射门区域内
	{
		pointToGo = findFurtherPos(env,whichRobot,predictedPos,4);//找球的落点
		sineTurn(pointToGo,whichRobot);//以曲线移动到落点
	}


	else//若都不带球
	{
		if(findDistance(env->home[whichRobot].pos,predictedPos) < AIM_RADIUS && isBehindBall2(env, whichRobot) > 0)//若球与球门位置小于射门半径并且机器人位于球后面
		{
			if(isBehindBall2(env, whichRobot) == 1)//机器人与球距离为1
			{
				sineTurn(posToGoal(env, whichRobot), whichRobot);//lex:hit ball n goal直接射门
			}
			else if(isBehindBall2(env,whichRobot) == 2)
			{
				sineTurn(upperGoalPos,whichRobot);
			}
			else if(isBehindBall2(env,whichRobot) == 3)
			{
				sineTurn(lowerGoalPos,whichRobot);
			}
		}

		else //if(isBehindBall(env, whichRobot) == 0)//lex: go to behind ball若机器人在球与对方球门之间，则要绕到球与本方球门之间
		{
			if(env->home[whichRobot].pos.x > predictedPos.x && env->home[whichRobot].pos.y > predictedPos.y)
			{
				orbit(predictedPos,&(env->home[whichRobot]),whichRobot,ORBIT_DISTANCE,ANTI_CLOCKWISE);
			}
			else if(env->home[whichRobot].pos.x > predictedPos.x && env->home[whichRobot].pos.y < predictedPos.y)
			{
				orbit(predictedPos,&(env->home[whichRobot]),whichRobot,ORBIT_DISTANCE,CLOCKWISE);
			}
			else
			{
				gradient = findGradient(predictedPos, oppoCentreGoal);
				pointToGo.y = gradient * (pointToGo.x - oppoCentreGoal.x) + oppoCentreGoal.y;
				sineTurn(pointToGo,whichRobot);
			}
		}
	//	else
	//	{
	//		spin(&env->home[whichRobot],125, 1);
	//	}
	}
	/*	FILE *pFile;
	pFile = fopen("C:\\STRATEGY\\predictfile.txt","a+");
	
		fprintf(pFile,"%f\t\t",env->currentBall.pos.x);
		//	fprintf(pFile,"%f\t\t", env->lastBall.pos.x);
	//	fprintf(pFile,"%f\t\t",env->currentBall.pos.x);
//		fprintf(pFile,"%f\n", predictedPos.x);		
	
	fclose(pFile);*/


	return;

}

Vector3D CAttacker::posToGoal(Environment *env, int whichRobot)//控制机器人移动到射门的点
{
	double gradient;
	Vector3D pointToGo;
	double howFar;
	double angle;
	howFar = predictHowFar(env, whichRobot);//use lex predict ball or not??

	pointToGo = predictedBall(env, howFar);
	pointToGo.x = FRIGHTX;
	gradient = findGradient(env->home[whichRobot].pos, env->currentBall.pos);
	pointToGo.y = (gradient * (pointToGo.x - env->home[whichRobot].pos.x)) + env->home[whichRobot].pos.y;
	if( pointToGo.y > GTOPY || pointToGo.y < GBOTY)
	{
		findFurtherPos(env,whichRobot,env->currentBall.pos,3);
	}
	

	return pointToGo;
}

bool CAttacker::isBehindBall(Environment *env, int whichRobot)//判断机器人是否在球后面，即是否位于本方球门和足球之间
{
	bool behind;
	double howFar;
	howFar = predictHowFar(env, whichRobot);

	double gradient1, gradient2;
	Vector3D GTOP, GBOT, tempPoint1, tempPoint2;
	Vector3D predictedPos;
	predictedPos = predictedBall(env, howFar);

	GTOP.x = FRIGHTX;
	GBOT.x = FRIGHTX;
	GTOP.y = GTOPY;
	GBOT.y = GBOTY;
	tempPoint1.x = env->home[whichRobot].pos.x;
	tempPoint2.x = env->home[whichRobot].pos.x;
	if(env->home[whichRobot].pos.x < predictedPos.x && (findDistance(env->home[whichRobot].pos,env->currentBall.pos)) < 4 && findDistance(env->home[whichRobot].pos,env->currentBall.pos) > 1.95)
	{
		behind = 1;
	}
	else if(findDistance(env->home[whichRobot].pos,env->currentBall.pos) > 4)
	{
		gradient1 = findGradient(predictedPos, GBOT);
		tempPoint1.y = gradient1 * (tempPoint1.x - predictedPos.x) + predictedPos.y;
		gradient2 = findGradient(predictedPos, GTOP);
		tempPoint2.y = gradient2 * (tempPoint2.x - predictedPos.x) + predictedPos.y;
		if(env->home[whichRobot].pos.y < tempPoint1.y && env->home[whichRobot].pos.y > tempPoint2.y)
		{
			behind = 1;
		}
		else
		{
			behind = 0;
		}
	}
	else
	{
		behind = 0;
	}
	return behind;
}

int CAttacker::isBehindBall2(Environment *env, int whichRobot)
{
	double gradient;
	Vector3D pointToGo,predictPos;
	double howFar;
	double angle;
	Vector3D upperGoalPos,lowerGoalPos;
	upperGoalPos.x = FRIGHTX;
	lowerGoalPos.x = FRIGHTX;
	upperGoalPos.y = 47.7151;
	lowerGoalPos.y = 35.5756;
	double ROBOT_WIDTH = 1.2883;
	int behind;
	howFar = predictHowFar(env, whichRobot);//use lex predict ball or not??

	predictPos = predictedBall(env, howFar);
	pointToGo.x = FRIGHTX;
	gradient = findGradient(env->home[whichRobot].pos,predictPos);
	pointToGo.y = (gradient * (pointToGo.x - env->home[whichRobot].pos.x)) + env->home[whichRobot].pos.y;
	if( pointToGo.y < GTOPY || pointToGo.y > GBOTY)
	{
		behind = 1;//可以直接射门
	}
	else if(isInBetweenPos(env,env->home[whichRobot].pos,upperGoalPos,predictPos,ROBOT_WIDTH) == 1)
	{
		behind = 2;//robot can shoot ball to upper of goal
	}

	else if(isInBetweenPos(env,env->home[whichRobot].pos,lowerGoalPos,predictPos,ROBOT_WIDTH) == 1)
	{
		behind = 3;//robot can shoot ball to lower of goal
	}
	else
	{
		behind = 0;
	}
	return behind;
}



void CAttacker::lexMidField(Environment *env, int whichRobot)
{
	Vector3D HOME_GOAL_CENTER_POSITION;
	HOME_GOAL_CENTER_POSITION.x = FLEFTX;
	HOME_GOAL_CENTER_POSITION.y = CENTERY;
	Vector3D oppoCentreGoal,pointToGo;
	oppoCentreGoal.x = FRIGHTX;
	oppoCentreGoal.y = CENTERY;
	Vector3D goalPosition;
	double tempY, C;
	double gradient, inverseGradient;
	if(env->currentBall.pos.x < 50)
	{
		goalPosition = HOME_GOAL_CENTER_POSITION;//Defender Mode
	}
	else
	{
		goalPosition = oppoCentreGoal;//Attacker Mode
		//this Mode should decide by role selection which ll done 
		//and this code should change
	}

	Vector3D AttackerPosition = env->home[env->userData->attacker].pos;//lex: hav to change which one is attacker, home[3] is only a temp vec3D
	gradient = findGradient(goalPosition,AttackerPosition);
	inverseGradient = 1 / gradient;
	C = goalPosition.y - (gradient * goalPosition.x);
	tempY = gradient * env->currentBall.pos.x + C;
	double angle;
	angle = findPointToPointAngle(goalPosition,env->currentBall.pos );


	/*

  Goalcentre   Attacker   | tempY,ball.y(x,y)
  * -------------*--------*----------
                          |
                          *Predict ball position
			
	
	
	*/
	if(env->currentBall.pos.y > tempY)
	{

//		C = env->home[env->userData->Attacker].pos.y - (inverseGradient * env->home[env->userData->Attacker].pos.x);
//		pointToGo.x = env->home[env->userData->Attacker].pos.x+ sin(angle*PI/180) * 15;
//		pointToGo.y = inverseGradient * PointToGo.x + C;
		pointToGo.x = env->home[env->userData->attacker].pos.x - 6;
		pointToGo.y = env->home[env->userData->attacker].pos.y +  12;
		sineTurn(pointToGo,whichRobot);
	//	avoidObstacle(env,whichRobot,pointToGo);
	}
	else
	{

//		pointToGo.x = env->home[env->userData->attacker].pos.x+ sin(angle*PI/180) * 15;
//		pointToGo.y = inverseGradient * PointToGo.x + C;
		pointToGo.x = env->home[env->userData->attacker].pos.x - 6;
		pointToGo.y = env->home[env->userData->attacker].pos.y - 12;
		sineTurn(pointToGo,whichRobot);
	//	avoidObstacle(env,whichRobot,pointToGo);
	}
		return;
}



void CAttacker::waiter(Environment *env, int whichRobot1, int whichRobot2)
{
	Vector3D temp,temp2;
	temp.x = 29;
	temp2.x = 29;
	temp.y = 64;
	temp2.y = 18;

		sineTurn(temp,whichRobot1);
		sineTurn(temp2,whichRobot2);
		return;
}