team.cpp

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

// Team.cpp: implementation of the Team class.
//
//////////////////////////////////////////////////////////////////////

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

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

Team::Team(Environment *envPointer)//构造函数
{
   env = envPointer;
   basicAction = new CBasicAction(envPointer);
   attacker = new CAttacker(envPointer);
   defender = new CDefender(envPointer);
   goalie = new CGoalie(envPointer);
   support = new CSupport(envPointer);
   sweeper = new CSweeper(envPointer);

}

Team::~Team()
{
	if (basicAction!=NULL) delete basicAction; //如果basicAction不空，则删除前一个basicAction
    if (attacker!=NULL) delete attacker;//同上
	if (defender!=NULL) delete defender;//同上
	if (goalie!=NULL) delete goalie;//同上
	if (support!=NULL) delete support;//同上
	if (sweeper!=NULL) delete sweeper;//同上
}

void Team::normalGame(Environment *env)//在没有其他以外情况下的踢球方式
{
	void findRobotLastPosition();//寻找足球的上一个位置
//-------------------------Test Sineturn---------------------------------
	/*	Vector3D testpos;
		Vector3D testpos2;

	testpos.x = 71.7305;
	testpos.y = 64.4898;
	testpos2.x = 71.7305;
	testpos2.y = 18.3483;

	basicAction->lexSineTurn(testpos,4);
	basicAction->sineTurn(testpos2,0,0,0,0,3);
	if(env->home[1].pos.x <71.7305)
	{
		env->home[1].velocityLeft = 127;
		env->home[1].velocityRight = 127;
	}
	else
		basicAction->brake(1);*/

//-------------------------------------------------------------
//-----------------Test Drible&Spin----------------------------

/*	
	if (env->home[3].pos.x<25)
	{
		env->home[3].velocityLeft=50;
		env->home[3].velocityRight=50;
	//	env->home[4].velocityLeft=125;
	//	env->home[4].velocityRight=125;

	}

	else
	{
		basicAction->brake(3);

    }

*/
//	basicAction->spin(&env->home[3],125,1);
	
//-----------------------------------------------------------------------------

/*	
	CAttacker A(env, UserData);
	CDefender D(env,UserData);
	CGoalie G(env,UserData);
	CSweeper Sw(env, UserData);
	CSupport Su(env, UserData);

//	A.OffensiveStrategem(env,env->predictedBall.pos,UserData->Attacker);
	A.lexAttacker(env,UserData->Attacker);
	A.lexMidField(env,UserData->Support1);
//	A.Waiter(env,UserData->Support1,UserData->Support2);
	D.lexDefenderNormal(env,UserData->Support2,UserData->Sweeper);
//	D.EstablishDefensivePerimeter2(env->predictedBall.pos.x,UserData->Support2,UserData->Support1,2);
//	Su.BlockInFrontOfGoal(UserData->Support1, UserData->Support2);
//	Sw.Sweeper(UserData->Sweeper);
	G.Goalie(&(env->home[0]),UserData->Goalie);*/
/*	if(attacker->isBehindBall(env,4)==1)
	{
		basicAction->spin(&env->home[0],125,CLOCKWISE);
	}
	if(attacker->isBehindBall(env,4)==0)
	{
		basicAction->brake(0);
	}*/

	attacker->lexAttacker(env,env->userData->attacker);

//	attacker->lexAttacker(env,4);

	attacker->lexMidField(env,env->userData->support1);
	defender->lexDefenderNormal(env,/*env->userData->support2,env->userData->sweeper*/1,2);
	goalie->goalie(&(env->home[0]),0);
	/*
		FILE *pFile;
	pFile = fopen("C:\\STRATEGY\\predictfile.txt","a+");
	
		fprintf(pFile,"%f\n",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;
}

void Team::freeKickMode(Environment *env)//一般状态下的踢球方式
{
		attacker->lexAttacker(env,env->userData->attacker);

//	attacker->lexAttacker(env,4);

	attacker->lexMidField(env,env->userData->support1);
	defender->freeKickDefender(env,/*env->userData->support2,env->userData->sweeper*/1,2);
	goalie->goalie(&(env->home[0]),0);
	return;
}

void Team::findRobotToBallDistance()//计算机器人到足球的距离
{

	double dx, dy;

	for(int i=0;i<5;i++)//分别计算5个机器人到足球的距离
	{
		dx = env->home[i].pos.x - env->currentBall.pos.x;//足球横坐标
		dy = env->home[i].pos.y - env->currentBall.pos.y;//足球纵坐标
		env->userData->HomeRobotData[i].toBall.distance = sqrt((dy * dy) + (dx * dx));//通过公式计算出距离
	}
	

	return;
}

void Team::findRobotToRobotDistance()//计算本方机器人之间的距离
{

	double dx, dy;

	for(int i=0;i<5;i++)//分别本方机器人之间的距离
	{
		for(int j=0;j<5;j++)
		{
			dx = env->home[i].pos.x - env->home[j].pos.x;
			dy = env->home[i].pos.y - env->home[j].pos.y;
		
			env->userData->HomeRobotData[i].toRobot[j].distance = sqrt((dy * dy) + (dx * dx));
		}
	}

	return;
}

void Team::findRobotToOppoRobotDistance()//计算本方机器人与对方机器人之间的距离
{

	double dx, dy;

	for(int i=0;i<5;i++)//分别计算本方机器人与对方机器人之间的距离
	{
		for(int j=0;j<5;j++)
		{
			dx = env->home[i].pos.x - env->opponent[j].pos.x;
			dy = env->home[i].pos.y - env->opponent[j].pos.y;
		
			env->userData->HomeRobotData[i].toOppo[j].distance = sqrt((dy * dy) + (dx * dx));
		}
	}

	return;
}

int Team::findClosestRobotToBall()//计算离球最近的机器人
{
	int closest = 0;
	for (int i=0; i<5; i++)
	{
		if (env->userData->HomeRobotData[closest].toBall.distance>env->userData->HomeRobotData[i].toBall.distance)//如果HomeRobotData[closest].toBall.distance与HomeRobotData[i].toBall.distance指向相同的量
			closest = i;//则closest = i
	}
	return closest;
}

int Team::findMostFarRobotToBall()//找离球最远的机器人（不包括守门员）
{
	//not including Goalie
	int futurest = 1;//假设最远的机器人的编号为1
	for (int i=2; i<5; i++)//循环4次（还有4个球员）
	{
		if (env->userData->HomeRobotData[futurest].toBall.distance < env->userData->HomeRobotData[i].toBall.distance)
			futurest = i;//如果有编号为i的机器人离球更远，则将离球最远的机器人设为i
	}
	return futurest;
}
int Team::findClosestRobotToRobot(int whichRobot)//计算离球最近的机器人（包括守门员）
{
	int closest = 0;
	if (whichRobot==0)//？？？
		closest=1; //When whichRobot is 0, cloest will be 0
	for (int i=0; i<5; i++)
	{
			
		if (env->userData->HomeRobotData[i].toRobot[whichRobot].distance < env->userData->HomeRobotData[closest].toRobot[whichRobot].distance
			&& env->userData->HomeRobotData[i].toRobot[whichRobot].distance!=0)
			closest = i;//方法同上
		
	}

	return closest;
}

int Team::findClosestRobotToOppoRobot(int whichRobot)//寻找最靠近对方球员的本方队员
{
	int closest = 0;
	if (whichRobot==0)
		closest=1; //When whichRobot is 0, cloest will be 0
	for (int i=0; i<5; i++)
	{
			
		if (env->userData->HomeRobotData[i].toOppo[whichRobot].distance < env->userData->HomeRobotData[closest].toOppo[whichRobot].distance)
			closest = i;
		
	}

	return closest;//方法同上
}

void Team::findRobotToBallAngle()//计算机器人和球的角度
{
//	CBasicAction BA(env, UserData);

	double dx, dy, desiredAngle, angleError;

	for(int i=0;i<5;i++)
	{
		for(int j=0;j<5;j++)//？？？
		{
			dx = env->home[i].pos.x - env->currentBall.pos.x;
			dy = env->home[i].pos.y - env->currentBall.pos.y;
			desiredAngle = 180/3.142 * atan2(dy, dx);
			angleError = desiredAngle - (double)env->home[i].rotation;
			angleError = angleNormalisation(angleError,0);

			env->userData->HomeRobotData[i].toBall.angle = angleError;
		}
	}

	return;
}


void Team::findRobotToRobotAngle()// 计算机器人之间的角度
{
//	CBasicAction BA(env, UserData);
	double dx, dy, desiredAngle, angleError;
	
	for(int i=0;i<5;i++)//分别计算每2个机器人之间的角度
	{
		for(int j=0;j<5;j++)
		{
			dx = env->home[i].pos.x - env->home[j].pos.x;
			dy = env->home[i].pos.y - env->home[j].pos.y;
			desiredAngle = 180/3.142 * atan2(dy, dx);
			angleError = desiredAngle - (double)env->home[i].rotation;
			angleError = angleNormalisation(angleError,1);

			//UserData->HomeRobotInfo[i] = angleError;
		}
	}	


	return;
}

void Team::findRobotToGoalAngle()//计算机器人和球门的角度
{
//	CBasicAction BA(env, UserData);
	
	double robotToOppoGoalCenterAngle;//机器人与球门中心的角度	
	double robotToBallAngle;//机器人与球的角度

	Vector3D OPPO_GOAL_CENTER;
	OPPO_GOAL_CENTER.x = FRIGHTX;
	OPPO_GOAL_CENTER.y = CENTERY;
	//FILE *pFile;
	//pFile = fopen("C:\\STRATEGY\\Opponent Data For Yellow.txt","w");
	for(int i=0;i<5;i++)
	{
		robotToOppoGoalCenterAngle = findPointToPointAngle(env->home[i].pos, OPPO_GOAL_CENTER);
		robotToBallAngle = findPointToPointAngle(env->home[i].pos, env->currentBall.pos);

			
		env->userData->HomeRobotData[i].GoalAngle = robotToOppoGoalCenterAngle - robotToBallAngle;
		//计算机器人与球门角度和机器人与球的角度之差			
	/*
		fprintf(pFile,"ToGoal %d = %f\n", i, robotToOppoGoalCenterAngle);
		fprintf(pFile,"ToBall %d = %f\n", i, robotToBallAngle);
		fprintf(pFile,"GoalAngle %d = %f\n", i, UserData->HomeRobotInfo[i].GoalAngle);
	*/
	
	}	
	//fclose(pFile);

	return;	
}
	
void Team::findRobotLastPosition()//求机器人的当前位置??
{
	static Vector3D sp_lastPos[5];
	for(int i=0;i<5;i++)
	{
		env->userData->HomeRobotData[i].lastPosition = sp_lastPos[i];
		sp_lastPos[i] = env->home[i].pos;
	}
}





void Team::faceAngle(int whichRobot)//根据前面算出的角度调整机器人自身的角度
{
//	CBasicAction BA(env, UserData);
	findRobotToRobotAngle();
	double angleError;
	for(int i=0;i<5;i++)
	{
		angleError = env->userData->HomeRobotData[i].toRobot[whichRobot].angle - (double)env->home[i].rotation;
		basicAction->turnAngle(i, angleError);
	}
	
	return;
}

void Team::detectOppoBlockBall()
{
//	CBasicAction BA(env, UserData);
	bool pathClear;
	for(int i=0;i<5;i++)
	{
		pathClear = basicAction->isPathClear(env, env->home[i].pos, env->predictedBall.pos);
		env->userData->HomeRobotData[i].ToOppoPathClear = pathClear ;
	}
	return;
}

void Team::assignRole(Environment *env)//场上角色的转换
{
	findGoForBallFactor();

	static int sp_max =4;
	static int sp_max2 = 3;
	int max3 = 0;
	int max4 = 0;

/*	for(int i=1;i<5;i++)
	{
		if (UserData->HomeRobotInfo[i].GoForBall > UserData->HomeRobotInfo[max].GoForBall)
		{
			max = i; 
		}
	}
	
	for(i=1;i<5;i++)
	{
		if (UserData->HomeRobotInfo[i].GoForBall > UserData->HomeRobotInfo[max2].GoForBall
			&& UserData->HomeRobotInfo[i].GoForBall < UserData->HomeRobotInfo[max].GoForBall)
		{
			max2 = i;
		}
	}
	
	for(i=1;i<5;i++)
	{
		if (UserData->HomeRobotInfo[i].GoForBall > UserData->HomeRobotInfo[max3].GoForBall
			&& UserData->HomeRobotInfo[i].GoForBall < UserData->HomeRobotInfo[max2].GoForBall)
		{
			max3 = i;
		}
	}


	for(i=1;i<5;i++)
	{
		if (UserData->HomeRobotInfo[i].GoForBall > UserData->HomeRobotInfo[max4].GoForBall
			&& UserData->HomeRobotInfo[i].GoForBall < UserData->HomeRobotInfo[max3].GoForBall)
		{
			max4 = i; 
		}
	}*/
//	if(abs(env->userData->HomeRobotData[4].GoForBall - env->userData->HomeRobotData[3].GoForBall) >1)
	{

		if (env->userData->HomeRobotData[4].GoForBall > env->userData->HomeRobotData[3].GoForBall)
		{
			sp_max = 4;
			sp_max2 = 3;
		}
		else
		{
			sp_max=3;
			sp_max2=4;	

		}
	}


	env->userData->attacker = sp_max;
	env->userData->support1 = sp_max2;
	env->userData->support2 = 2;
	env->userData->sweeper = 1;
	env->userData->goalie = 0;

/*	FILE *pFile;
	pFile = fopen("C:\\STRATEGY\\Role.txt","a+");
	
	fprintf(pFile,"Atteacker = %d\n", env->userData->attacker);
	fprintf(pFile,"Support1 = %d\n", env->userData->support1);
	fprintf(pFile,"Support2 = %d\n", env->userData->support2);
	fprintf(pFile,"Sweeper = %d\n", env->userData->sweeper);
	fprintf(pFile,"Goalie = %d\n", env->userData->goalie);

	
	fclose(pFile);*/


	return;
}

void Team::findGoForBallFactor()//计算最适合射门的球员
{
	findRobotToBallDistance();
	findRobotToBallAngle();
	findRobotToGoalAngle();
	detectOppoBlockBall();
	
	int furturestRobot = findMostFarRobotToBall();
	double distanceFactor, distanceWeight;
	double angleFactor, angleWeight;
	double goalAngle, goalAngleWeight;
	bool oppoBlockFactor;
	double oppoBlockWeight;
	distanceWeight = 8;
	angleWeight = 3;
	goalAngleWeight = 5;
	oppoBlockWeight = 6;
	
	env->userData->HomeRobotData[0].GoForBall = -1; //Goalie do not go for ball

//	FILE *pFile;
//	pFile = fopen("C:\\STRATEGY\\GoForBall.txt","w");
	for (int i=1; i<5; i++)
	{
		distanceFactor = 1 - (env->userData->HomeRobotData[i].toBall.distance / env->userData->HomeRobotData[furturestRobot].toBall.distance);
		angleFactor = cos(3.142/180 * env->userData->HomeRobotData[i].toBall.angle);
		goalAngle = cos(3.142/180 * env->userData->HomeRobotData[i].GoalAngle);
		oppoBlockFactor = env->userData->HomeRobotData[i].ToOppoPathClear;


		env->userData->HomeRobotData[i].GoForBall = (distanceFactor * distanceWeight + angleFactor * angleWeight + goalAngle * goalAngleWeight + oppoBlockFactor * oppoBlockWeight)
			/(distanceWeight+angleWeight+goalAngleWeight+oppoBlockWeight);
		
		//fprintf(pFile,"distance %d = %f\n", i, UserData->HomeRobotInfo[i].toBall.distance);
//		fprintf(pFile,"fd %d = %f\n", i, distanceFactor);
		//fprintf(pFile,"angle %d = %f\n", i, UserData->HomeRobotInfo[i].toBall.angle);
//		fprintf(pFile,"ad %d = %f\n", i, angleFactor);
		//fprintf(pFile,"GoalAngle %d = %f\n", i, UserData->HomeRobotInfo[i].GoalAngle);
//		fprintf(pFile,"GoalFactor %d = %f\n", i, GoalAngle);
//		fprintf(pFile,"Factor %d = %f\n", i, UserData->HomeRobotInfo[i].GoForBall);
	}
	
//	fclose(pFile);

	
	return;
}