test.cpp

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

// Test.cpp: implementation of the CTest class.
//
//////////////////////////////////////////////////////////////////////

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

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

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

CTest::~CTest()
{

}




void CTest::Spin()
{
	CBasicAction BA(env, UserData);
	Vector3D test;
	test.x = env->home[0].pos.x;
	if(env->home[0].pos.y > 57)
	{
		test.y = 0;
		BA.Position(test,0);
	}
	
	if(env->home[0].pos.y < 25)
	{
		test.y = 100;
		BA.Position(test,0);
	}
	
	return;
}


bool CTest::Push(int WhichRobot)
{
	for(int i=0;i<5;i++)
	{
		/*
		FILE *pFile;
		pFile = fopen("C:\\STRATEGY\\Opponent Data For Yellow.txt","a+");
		fprintf(pFile,"Robot[4] to oppt[%d] = %f\n",i, UserData->HomeRobotInfo[0].toOppo[i].hypo);

		fclose(pFile);
		*/
		if (UserData->HomeRobotInfo[WhichRobot].toOppo[i].hypo<=ROBOTRADIUS*0.8)
		{
			//if (Direction(&env->home[WhichRobot]))
			{
				return true;
			}
		}
	}


	return false;
}

int CTest::Direction(Robot *robot)
{
	static Vector3D oldPos;
	static counter;
	double dx, dy;
	int quad;
	if (counter==0) oldPos = robot->pos;
	counter++;
	if (counter==5) counter = 0;
	
	dx = robot->pos.x - oldPos.x;
	dy = robot->pos.y - oldPos.y;
	if(dx>0 && dy>0) quad = 1;
	if(dx<0 && dy>0) quad = 2;
	if(dx<0 && dy<0) quad = 3;
	if(dx>0 && dy<0) quad = 4;
/*
	FILE *pFile;
	pFile = fopen("C:\\STRATEGY\\Opponent Data For Yellow.txt","a+");
	fprintf(pFile,"old = %f\n",oldPos.x);
	fprintf(pFile,"now = %f\n",robot->pos.x);
	fprintf(pFile,"dx = %f\n",dx);
	fprintf(pFile,"dy = %f\n",dy);
	fprintf(pFile,"quad = %d\n",quad);
	fclose(pFile);
*/
	return quad;

}

double CTest::Predict_Ball(int WhichRobot)
{
	static Vector3D oldPos;
	static counter;
	double dx, dy;
	static double distance;
	double oldDistance, velocity;
	
	
	if (counter==0) oldPos = env->home[WhichRobot].pos;
	counter++;
	if (counter==5) counter = 0;
	
	dx = env->home[WhichRobot].pos.x - oldPos.x;
	dy = env->home[WhichRobot].pos.y - oldPos.y;
	
	oldDistance = distance;
	distance = sqrt((dy * dy) + (dx * dx));

	velocity = distance - oldDistance;
	return velocity;

}

double CTest::Gradient(Vector3D p1, Vector3D p2)
{
	double dx, dy, g;

	dx = p1.x - p2.x;
	dy = p1.y - p2.y;

	g = dy/dx;
	return g;
}



void CTest::Trap_Ball(int WhichRobot, int WhichRobot2)
{
	
	CBasicAction BA(env, UserData);
	
	

	static Vector3D oldBallPos;
	Vector3D block_point, offset, offset2;
	static counter;
	double dx, dy;



	
	//if(UserData->HomeRobotInfo[WhichRobot].toBall.hypo>ROBOTRADIUS)
	{


		if (counter==0) oldBallPos = env->currentBall.pos;
		counter++;
		if (counter==10) counter = 0;
		
		
		dx = env->currentBall.pos.x - oldBallPos.x;
		dy = env->currentBall.pos.y - oldBallPos.y;
		
		block_point.x = 40;//env->currentBall.pos.x + dx*2;
		block_point.y = 40;//env->currentBall.pos.x + dy*2;

		offset.x = block_point.x - ROBOTRADIUS/2;
		offset.y = block_point.y - ROBOTRADIUS/2;

		offset2.x = block_point.x + ROBOTRADIUS/2;
		offset2.y = block_point.y + ROBOTRADIUS/2;

		BA.Position(offset, WhichRobot);
		BA.Position(offset2, WhichRobot2);


	}
	//else
	{
		//BA.Brake(WhichRobot);
		//BA.Brake(WhichRobot2);
	}
		
/*	FILE *pFile;
	pFile = fopen("C:\\STRATEGY\\Opponent Data For Yellow.txt","a+");
	fprintf(pFile,"x = %f, y = %f\n", block_point.x, block_point.y);
	fclose(pFile);*/

	return;
}

void CTest::Test(int WhichRobot)
{
	
	CBasicAction BA(env, UserData);


	//BA.Position(env->currentBall.pos, WhichRobot);
	
	
	if (Push(WhichRobot)==true)
		BA.Spin(&env->home[0], 125, 1);
	else
		BA.Brake(0);
	
		
	/*
	
	if (Push(WhichRobot)==false)
	{
		BA.Position(env->currentBall.pos, WhichRobot);
	}
	else
	{
		//BA.Hook(&env->home[WhichRobot], 1);
		BA.Brake(WhichRobot);
	}
	*/


}

void CTest::Penalty_Kick(int WhichRobot)
{
	CBasicAction BA(env, UserData);
	CChaseAction CA(env, UserData);

	double RobotFront = BA.Decide_Robot_Front(env->home[WhichRobot].rotation);

	double dx2 = env->home[WhichRobot].pos.x - env->predictedBall.pos.x;
	double dy2 = env->home[WhichRobot].pos.y - env->predictedBall.pos.y;
	
	double CurrentAngle = 180/3.142 * atan2((double)(dy2), (double)(dx2));
	if(CurrentAngle < 0)
	{
		CurrentAngle +=360;
	}

	double Angle = CurrentAngle;


	if((env->currentBall.pos.x - env->home[WhichRobot].pos.x) > 0 //behind ball
		&& (env->currentBall.pos.x - env->home[WhichRobot].pos.x) < 4.0 //distance more then 4
		&& (env->home[WhichRobot].pos.y - env->currentBall.pos.y) < 1.5 // y more then 1.5
		&& (-(env->home[WhichRobot].pos.y - env->currentBall.pos.y)) < 1.5)

	//if(env->home[WhichRobot].pos.x>100 )
	{
		BA.Spin(&(env->home[WhichRobot]),127,CLOCKWISE);
	}
	else
	{
		if(Angle<230.0&& Angle>130.0)
		{
			//env->home[WhichRobot].velocityLeft = RobotFront*125;
			//env->home[WhichRobot].velocityRight = RobotFront*125;
			
			BA.SineTurn(env->predictedBall.pos, 127, 1, 0, 1, WhichRobot);
		}
		else // robot not in position
		{
			CA.Orbit(env->predictedBall.pos,&(env->home[WhichRobot]),WhichRobot,5,CLOCKWISE);
		}
		
	}
	


}


void CTest::Goalie (int WhichRobot)
{
	CBasicAction BA(env, UserData);
	CChaseAction CA(env, UserData);

	double RobotFront;

	RobotFront = BA.Decide_Robot_Front(env->home[WhichRobot].rotation);
	
	if (env->home[WhichRobot].pos.x >18)
	{
		env->home[WhichRobot].velocityLeft = -RobotFront*100;
		env->home[WhichRobot].velocityRight = -RobotFront*100;
		
	}
	else
	{
		env->home[WhichRobot].velocityLeft = RobotFront*125;
		env->home[WhichRobot].velocityRight = RobotFront*125;
	}
	
/*
	double Vl = 0, Vr = 0;
	
	Vector3D Current_Ball_Position;
	Vector3D Robot_Position;
	Vector3D Point_To_Go;
	
	Current_Ball_Position.x = env->currentBall.pos.x;
	Current_Ball_Position.y = env->currentBall.pos.y;
	
	
	Robot_Position.x = env->home[WhichRobot].pos.x;
	Robot_Position.y = env->home[WhichRobot].pos.y;

	double Robot_Angle = env->home[WhichRobot].rotation;

	env->home[WhichRobot].velocityLeft = 0;
	env->home[WhichRobot].velocityRight = 0;


		
	if(Robot_Position.x >= 9.3 && Robot_Position.x < 10.8 && env->currentBall.pos.x < 70 )
	{
		
		if ( Robot_Position.y > env->currentBall.pos.y + 1.5 && env->home[WhichRobot].pos.y > 36 )
		{
			Vl = -100 ;
			Vr = -100 ;
		}

		if ( Robot_Position.y < env->currentBall.pos.y - 1.5 && env->home[WhichRobot].pos.y < 48 )
		{
			Vl = 100 ;
			Vr = 100 ;
		}
	

		if ( Robot_Position.y <= 36)
		{
			Point_To_Go.x = 10;
			Point_To_Go.y = 36;
			BA.Position(Point_To_Go, WhichRobot);
		}

		if ( Robot_Position.y >= 48 )
		{
			Point_To_Go.x = 10;
			Point_To_Go.y = 48;
			BA.Position(Point_To_Go, WhichRobot);
		}

		

		double T_Robot_Angle = env->home[WhichRobot].rotation - 180;
		if ( T_Robot_Angle < 0.001 )
			T_Robot_Angle = T_Robot_Angle + 360;
		if ( T_Robot_Angle > 360.001 )
			T_Robot_Angle = T_Robot_Angle - 360;
		if ( T_Robot_Angle > 270.5 )
			Vl = Vl + fabs ( T_Robot_Angle - 270 );
		else if ( T_Robot_Angle < 269.5 )
			Vr = Vr + fabs ( T_Robot_Angle - 270 );
			
		env->home[WhichRobot].velocityLeft = Vl;//velocityLeft;
		env->home[WhichRobot].velocityRight = Vr;//velocityRight;
	
	}
	

	else
	{
		Point_To_Go.x = 10;
		Point_To_Go.y = 42;
		BA.Position(Point_To_Go, WhichRobot);

	}
	
*/
	return;
	

}