angledistance.cpp

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

// AngleDistance.cpp: implementation of the CAngleDistance class.
//
//////////////////////////////////////////////////////////////////////

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


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

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

CAngleDistance::~CAngleDistance()
{

}

/*
void CAngleDistance::Spin(int WhichRobot)
{
	env->home[WhichRobot].velocityLeft = 125;
	env->home[WhichRobot].velocityRight = -125;
	return;
}
*/

void CAngleDistance::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 CAngleDistance::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 CAngleDistance::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 CAngleDistance::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 CAngleDistance::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 CAngleDistance::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 CAngleDistance::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 CAngleDistance::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 CAngleDistance::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 CAngleDistance::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 CAngleDistance::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;
}