coach.cpp

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

// Coach.cpp: implementation of the CCoach class.
//
//////////////////////////////////////////////////////////////////////

#include "stdafx.h"
#include "Coach.h"
#include "BasicAction.h"
#include "math.h"

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

CCoach::CCoach(Environment *envPointer, UserDefStruct *UserDataPointer)
{
	UserData = UserDataPointer;
	env = envPointer;
}

CCoach::~CCoach()
{

}

void CCoach::CalculateRobotToBallDistance()
{

	double dx, dy;

	for(int i=0;i<5;i++)
	{
		dx = env->home[i].pos.x - env->currentBall.pos.x;
		dy = env->home[i].pos.y - env->currentBall.pos.y;
		UserData->HomeRobotInfo[i].toBall.hypo = sqrt((dy * dy) + (dx * dx));
	}
	

	return;
}

void CCoach::CalculateRobotToRobotDistance()
{

	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;
		
			UserData->HomeRobotInfo[i].toRobot[j].hypo = sqrt((dy * dy) + (dx * dx));
		}
	}

	return;
}

void CCoach::CalculateRobotToOppoRobotDistance()
{

	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;
		
			UserData->HomeRobotInfo[i].toOppo[j].hypo = sqrt((dy * dy) + (dx * dx));
		}
	}

	return;
}

int CCoach::FindClosestRobottoBall()
{
	int closest = 0;
	for (int i=0; i<5; i++)
	{
		if (UserData->HomeRobotInfo[closest].toBall.hypo>UserData->HomeRobotInfo[i].toBall.hypo)
			closest = i;
	}
	return closest;
}

int CCoach::FindFuturestRobottoBall()
{
	//not including Goalie
	int futurest = 1;
	for (int i=2; i<5; i++)
	{
		if (UserData->HomeRobotInfo[futurest].toBall.hypo < UserData->HomeRobotInfo[i].toBall.hypo)
			futurest = i;
	}
	return futurest;
}
int CCoach::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 (UserData->HomeRobotInfo[i].toRobot[WhichRobot].hypo < UserData->HomeRobotInfo[closest].toRobot[WhichRobot].hypo
			&& UserData->HomeRobotInfo[i].toRobot[WhichRobot].hypo!=0)
			closest = i;
		
	}

	return closest;
}

int CCoach::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 (UserData->HomeRobotInfo[i].toOppo[WhichRobot].hypo < UserData->HomeRobotInfo[closest].toOppo[WhichRobot].hypo)
			closest = i;
		
	}

	return closest;
}

void CCoach::CalculateRobotToBallAngle()
{
	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 = BA.Angle_Normalisation(AngleError,0);

			UserData->HomeRobotInfo[i].toBall.angle = AngleError;
		}
	}

	return;
}


void CCoach::CalculateRobotToRobotAngle()
{
	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->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 = BA.Angle_Normalisation(AngleError,1);

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


	return;
}

void CCoach::CalculateRobotToGoalAngle()
{
	CBasicAction BA(env, UserData);
	
	double RobotToOppoGoalCenterAngle;
	double RobotToBallAngle;

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

			
		UserData->HomeRobotInfo[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 CCoach::FaceAngle(int WhichRobot)
{
	CBasicAction BA(env, UserData);
	CalculateRobotToRobotAngle();
	double AngleError;
	for(int i=0;i<5;i++)
	{
		AngleError = UserData->HomeRobotInfo[i].toRobot[WhichRobot].angle - (double)env->home[i].rotation;
		BA.TurnAngle(i, AngleError);
	}
	
	return;
}

void CCoach::DetectOppoBlockBall()
{
	CBasicAction BA(env, UserData);
	bool PathClear;
	for(int i=0;i<5;i++)
	{
		PathClear = BA.CheckPathClear(env, env->home[i].pos, env->predictedBall.pos);
		PathClear = UserData->HomeRobotInfo[i].ToOppoPathClear;
	}
	return;
}

void CCoach::AssginRole(Environment *env, UserDefStruct *UserData)
{
	CalculateGoForBallFactor();

	int max = 0;
	int max2 = 0;
	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 (UserData->HomeRobotInfo[4].GoForBall > UserData->HomeRobotInfo[3].GoForBall)
	{
		max = 4;
		max2 = 3;
	}
	else
	{
		max=3;
		max2=4;
	}


	UserData->Attacker = max;
	UserData->Support1 = max2;
	UserData->Support2 = 2;
	UserData->Sweeper = 1;
	UserData->Goalie = 0;

/*	FILE *pFile;
	pFile = fopen("C:\\STRATEGY\\Role.txt","a+");
	
	fprintf(pFile,"Atteacker = %d\n", UserData->Attacker);
	fprintf(pFile,"Support1 = %d\n", UserData->Support1);
	fprintf(pFile,"Support2 = %d\n", UserData->Support2);
	fprintf(pFile,"Sweeper = %d\n", UserData->Sweeper);
	fprintf(pFile,"Goalie = %d\n", UserData->Goalie);

	
	fclose(pFile);*/


	return;
}

void CCoach::CalculateGoForBallFactor()
{
	CalculateRobotToBallDistance();
	CalculateRobotToBallAngle();
	CalculateRobotToGoalAngle();
	DetectOppoBlockBall();
	
	int furturestRobot = FindFuturestRobottoBall();
	double DistanceFactor, DistanceWeight;
	double AngleFactor, AngleWeight;
	double GoalAngle, GoalAngleWeight;
	bool OppoBlockFactor;
	double OppoBlockWeight;
	DistanceWeight = 10;
	AngleWeight = 3;
	GoalAngleWeight = 0;
	OppoBlockWeight = 10;
	
	UserData->HomeRobotInfo[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 - (UserData->HomeRobotInfo[i].toBall.hypo / UserData->HomeRobotInfo[furturestRobot].toBall.hypo);
		AngleFactor = cos(3.142/180 * UserData->HomeRobotInfo[i].toBall.angle);
		GoalAngle = cos(3.142/180 * UserData->HomeRobotInfo[i].GoalAngle);
		OppoBlockFactor = UserData->HomeRobotInfo[i].ToOppoPathClear;


		UserData->HomeRobotInfo[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.hypo);
//		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;
}