strategy.cpp

5V5仿真平台机器人足球

/*  1. C++程序原则上被分写在两类文件中：
		1) 类的声明部分--写在“头文件”(扩展名.h) 中;
		2) 类的定义部分--写在“源文件”(扩展名.CPP) 中;
			C++源程序至少由一个源文件组成，源文件扩展名必须是CPP。Strategy.cpp是C++源程序文件名，
它是用户编写的机器人足球策略程序文件。

	2. 文件名不区分大小写，但C++源文件的内容所涉及的名字（函数名、变量名、数组名、常量名等）
严格区分大小写。

	3. C++源文件的开头部分一般是相应“头文件”编译指令（以#include开头，一条指令占一行），
作用是在编译时将“头文件”插入到指令所在位置一起编译。

	4. C++源文件由函数组成。在“头文件”编译指令之后，是函数声明和函数定义。
	  （注：在表示书写格式时，尖括号 < > 表示其括起部分内容由用户确定，其本身不是语法成分；
			下划线 _ 代表至少一个空格。
			方括号 [ ] 括起的部分有时可省略）

		1) 函数定义：
			(a) 函数头 格式

				<返回值类型>_<函数名>(<形参1类型>_<形参1名>, ...... , <形参n类型>_<形参n名>)

			(b) 函数体 格式

				{
					变量定义部分；
					语句组；
				}

			说明：
			(1) 各函数定义的先后次序无关紧要。
			(2) 主函数的函数名规定为 main()。程序总是首先执行主函数。主函数可调用任何其它函数，
			但不能被其它函数调用。除主函数外，函数可相互调用，也可自己调用自己。

		2) 函数声明：格式

				<返回值类型>_<函数名>(<参数1类型>[_<参数1名>], ...... , <参数n类型>[_<参数n名>])；

			说明：
			(1) 函数声明的格式与对应定义的函数头一致，但以分号结束；参数名可省略；
			(2) 函数声明出现在所有函数定义之前；
			(3) 为什么要函数声明？为了编译的需要。函数可相互调用，假如函数A调用了函数B，而函数A
			的定义先于函数B的定义，当编译到函数A时编译系统不“认识”函数B，导致编译失败。为此，
			在所以函数定义之前须对被调用的函数进行声明（事先通知编译系统该函数的名称、参数个数、
			参数类型及返回值类型）。为了方便可靠，建议除主函数外，一律对函数进行声明。
*/

//头文件////////////////////////////////////////////////////////
#include "stdafx.h"
#include "Strategy.h"
#include <math.h>
///////////////////////////////////////////////////////////////

//函数定义（此函数没有被本文件调用）/////////////////////////////////////////////
BOOL APIENTRY DllMain( HANDLE hModule, 
                       DWORD  ul_reason_for_call, 
                       LPVOID lpReserved
					 )
{
    switch (ul_reason_for_call)
	{
		case DLL_PROCESS_ATTACH:
		case DLL_THREAD_ATTACH:
		case DLL_THREAD_DETACH:
		case DLL_PROCESS_DETACH:
			break;
    }
    return TRUE;
}
////////////////////////////////////////////////////////////////

//以下为本文件定义的静态全局变量（主要用于对比赛各种状态的记录和识别）////////////////////////
static int kickStep[5]={0,0,0,0,0}, kickState[5]={0,0,0,0,0};
static double last_turn_angle[5]={0,0,0,0,0};
static double last_distance[5]={0,0,0,0,0};
/////////////////////////////////////////////////////////////////////////////////////////////////////

//以下为函数声明///////////////////////////////////////////////////////////
void Toward(int which, Vector2D point, double speed, Environment *env);
void PositAvoidBlock(int which, Vector2D target, Environment *env);
void Posit( int which, Vector2D point, double speed, Environment *env);
void Goalie_Act(int which, Environment *env);
void GoaliePosit( int which, Vector2D point, Environment *env);
void ReceiveData(Environment *env);
void PreserveData( );
void Velocity ( int which, double vl, double vr, Environment *env );
void AttackAvoidBlock(int which, Vector2D point, Environment *env);
void AvoidBlock(int which,Vector2D point, Environment *env);
void Form311(Environment *env);
void Determine_The_KickPit_And_TargetPit();
void Determine_The_Assistant_Points(Environment *env);
void Determine_Defend_Point();
void Assign_Tasks_To_Team_Members(Environment *env);
void Assistant_Act(int which, Vector2D point, Environment *env);
void Defend_Act(int which, Vector2D point, Environment *env);
void Attack_Act(int which, Environment *env);
/////////////////////////////////////////////////////////

/* Create(Environment *env)是一个外部函数，即只能被其它文件调用的函数。
它被比赛系统在比赛开始时调用一次，作用是创建本队策略程序所需要的数据结构。
*/
extern "C" STRATEGY_API void Create ( Environment *env )
{
}

/* Destroy(Environment *env)是一个外部函数，即只能被其它文件调用的函数。
它被比赛系统在比赛结束前调用一次，作用是释放由Create创建的数据结构所占内存空间。
*/
extern "C" STRATEGY_API void Destroy ( Environment *env )
{
}

/* Strategy(Environment *env)是一个外部函数，即只能被其它文件调用的函数。
它被比赛系统在比赛过程中反复调用（大约60毫秒调用一次），作用是实现用户的比赛策略。
*/
extern "C" STRATEGY_API void Strategy ( Environment *env )
{
	ReceiveData(env);//接收数据函数（相当于通过视觉系统获取动点（机器人和球）的坐标和前进角度
	
	switch (env->gameState)//根据比赛状态决定比赛策略
	{
		case 0:// default--正常比赛策略
			Form311(env);
			break;
		case FREE_BALL://争球策略
			Form311(env);
			break;
		case PLACE_KICK://开球策略
			Form311(env);
			break;			
		case PENALTY_KICK://罚点球策略
			Form311(env);
			break;
		case FREE_KICK://自由球策略
			Form311(env);
			break;
		case GOAL_KICK://球门球策略
			Form311(env);
			break;
	}
	PreserveData();	//保存当前周期的必要数据供下一周期使用
}

/////////////////////////////////////////////////////////
/*
	Velocity函数是本程序与比赛系统的接口：
	int which -- 我方机器人编号（0 ~ 4）；
	double vl -- 下周期施加于编号which的机器人左轮的力值（0 ~ 125）；
	double vr -- 下周期施加于编号which的机器人右轮的力值（0 ~ 125）；
	Environment *env -- 指向系统数据区的首地址的指针；
	将计算结果vl, vr的值传递给比赛系统数据区供其仿真。
*/
void Velocity ( int which, double vl, double vr, Environment *env )
{
	env->home[which].velocityLeft = vl;
	env->home[which].velocityRight = vr;
}
//////////////////////////////////////////////////////

void Posit( int which, Vector2D point, double speed, Environment *env)
{
	int robot_direct=1;

	double dprx = point.x - zw.home[which].pos.x;
	double dpry = point.y - zw.home[which].pos.y;
	double distance_pr = sqrt(dprx*dprx+dpry*dpry);

	double dbrx = zw.ball.pos.x - zw.home[which].pos.x;
	double dbry = zw.ball.pos.y - zw.home[which].pos.y;
	double distance_br = sqrt(dbrx*dbrx+dbry*dbry);

	double desired_angle;
	if(distance_pr<1*cm)
		desired_angle= 180.0*atan2(dbry, dbrx)/PI;
	else
		desired_angle= 180.0*atan2(dpry, dprx)/PI;
	
	double turn_angle=desired_angle - zw.home[which].ang;

	while(turn_angle > 180)
		turn_angle -= 360;
	while(turn_angle < -180)
		turn_angle += 360;
	
	if(turn_angle>90)
	{
		turn_angle-=180;
		robot_direct=-robot_direct;
	}
	else if(turn_angle<-90)
	{
		turn_angle+=180;
		robot_direct=-robot_direct;
	}

	double Adjust, d_ang;

	if(fabs(turn_angle)<1)
		d_ang=0;
	else
		d_ang=turn_angle-last_turn_angle[which];

	Adjust=1*turn_angle+2*d_ang;

	if(Adjust>speed)
		Adjust=speed;
	else if(Adjust<-speed)
		Adjust=-speed;

	double force=robot_direct*(speed-fabs(Adjust));

	double force_l, force_r;

	if(distance_pr<2.3*zw.home[which].dv)// || distance_pr>last_distance[which])
	{
		force_l = 0;
		force_r = 0;
	}
	else if(distance_pr<1.7*cm)
	{
		force_l = robot_direct*20*distance_pr-Adjust;
		force_r = robot_direct*20*distance_pr+Adjust;
	}
	else
	{
		force_l = force-Adjust;
		force_r = force+Adjust;
	}

	Velocity(which,force_l,force_r,env);

	last_turn_angle[which]=turn_angle;
	last_distance[which]=distance_pr;
}
//////////////04.02.07//////////////////////////////////////
void GoaliePosit( int which, Vector2D point, Environment *env)
{
	int robot_direct=1;

	double dprx = point.x - zw.home[which].pos.x;
	double dpry = point.y - zw.home[which].pos.y;
	double distance_pr = sqrt(dprx*dprx+dpry*dpry);

	double dbrx = zw.ball.pos.x - zw.home[which].pos.x;
//	double dbry = zw.ball.pos.y - zw.home[which].pos.y;
//	double distance_br = sqrt(dbrx*dbrx+dbry*dbry);

	double desired_angle;

	if(distance_pr<1.8*cm)
		desired_angle= 90;
	else
		desired_angle= 180.0*atan2(dpry, dprx)/PI;
	
	double turn_angle = desired_angle - zw.home[which].ang;

	while(turn_angle > 180)
		turn_angle -= 360;
	while(turn_angle < -180)
		turn_angle += 360;
	
	if(turn_angle>90)
	{
		turn_angle-=180;
		robot_direct=-robot_direct;
	}
	else if(turn_angle<-90)
	{
		turn_angle+=180;
		robot_direct=-robot_direct;
	}

	double Adjust, d_ang;

	if(fabs(turn_angle)<1)
		d_ang=0;
	else
		d_ang=turn_angle-last_turn_angle[which];

	Adjust=1*turn_angle+2*d_ang;

	if(Adjust>Vspeed) 
		Adjust=Vspeed;
	else if(Adjust<-Vspeed) 
		Adjust=-Vspeed;

	double force=robot_direct*(Vspeed-fabs(Adjust));
	double force_l, force_r;

	if(distance_pr<2.2*zw.home[which].dv || fabs(turn_angle)<4 && dpry*zw.home[which].dy<0 && zw.home[which].dv>0.01)// && zw.home[which].pos.y>GateB+4*cm && zw.home[which].pos.y<GateT-4*cm)
	{
		force_l = 0;
		force_r = 0;
	}
	else if(distance_pr<1.5*cm)
	{
		force_l = robot_direct*20*distance_pr-Adjust;
		force_r = robot_direct*20*distance_pr+Adjust;
	}
	else
	{
		force_l = force-Adjust;
		force_r = force+Adjust;
	}

	Velocity(which,force_l,force_r,env);

	last_turn_angle[which]=turn_angle;
	last_distance[which]=distance_pr;
}

////////////////////////////////////////////////////////////////////////

void Toward( int which, Vector2D point, double speed, Environment *env)
{
	int Go_ahead=1;

	double dprx = point.x - zw.home[which].pos.x;
	double dpry = point.y - zw.home[which].pos.y;
	                                            
	double distance_pr = sqrt(dprx*dprx+dpry*dpry);	                                            

	double desired_angle = 180.0*atan2(dpry, dprx)/PI;
	
	double turn_angle=desired_angle - zw.home[which].ang;

	while(turn_angle > 180)
		turn_angle -= 360;
	while(turn_angle < -180)
		turn_angle += 360;

	if(turn_angle>90)
	{
		turn_angle-=180;
		Go_ahead=-Go_ahead;
	}
	else if(turn_angle<-90)
	{
		turn_angle+=180;
		Go_ahead=-Go_ahead;
	}

	
	double Adjust, d_ang;
	if(fabs(turn_angle)<1)
		d_ang=0;
	else
		d_ang=turn_angle-last_turn_angle[which];

	Adjust=1*turn_angle+2*d_ang;

	if(Adjust>speed) 
		Adjust=speed;
	else if(Adjust<-speed) 
		Adjust=-speed;

	double force=Go_ahead*(speed-fabs(Adjust));

	double force_l = force-Adjust;
	double force_r = force+Adjust;

	Velocity(which,force_l,force_r,env);

	last_turn_angle[which]=turn_angle;
}

///////////////////////////////////////////////////////////////////////////////
///////////////////////////////////////////////////////////////
void Form311(Environment *env)
{
//计算各球员场上战位目标点
	Determine_The_KickPit_And_TargetPit();//计算踢球队员占位点和踢球方向
	Determine_The_Assistant_Points(env);//计算两助攻队员的占位点
	Determine_Defend_Point() ;//计算防守队员的占位点

//角色分配
	Assign_Tasks_To_Team_Members(env);//分配角色

//动作实现
	AttackAvoidBlock(zw.attacker, zw.kickPit,  env);
//	Attack_Act(zw.attacker, env);//进攻队员实现进攻动作
	Assistant_Act(zw.assist1, zw.assist1_pos, env);//第一助攻队员实现动作
	Assistant_Act(zw.assist2, zw.assist2_pos, env);//第二助攻队员实现动作
	Defend_Act(zw.defender, zw.defend_pos, env);//防守队员实现动作
	Goalie_Act(zw.goalie, env);//守门员动作
}
////////////////////////////////////////////////////////////

//计算进攻队员的占位目标点和踢球目标点///////////////////////////////////////////////
void Determine_The_KickPit_And_TargetPit()
{
	double dtbx, dtby, dist_tb;

	zw.target.x=FLEFT;//进攻队员踢球目标点
	zw.target.y=FMIDY;

	dtbx=zw.target.x-zw.ball.pos.x;
	dtby=zw.target.y-zw.ball.pos.y;
	dist_tb=sqrt(dtbx*dtbx+dtby*dtby);

	zw.kickPit.x=zw.ball.pos.x-R10*dtbx/dist_tb;//进攻队员转身踢球点
	zw.kickPit.y=zw.ball.pos.y-R10*dtby/dist_tb;
}
////////////////////////////////////////////////////////////

void Determine_The_Assistant_Points(Environment *env)
{
	Vector2D assist_pos1, assist_pos2;

	double Bal_to_L=zw.ball.pos.x-FLEFT;
	double Bal_to_B=zw.ball.pos.y-FBOTTOM;
	double T_to_Bal=FTOP-zw.ball.pos.y;
	double R_to_Bal=FRIGHT-zw.ball.pos.x;

	if(zw.ball.dx>=0)
	{
		if(zw.ball.pos.x<PenalL)
		{
			if(zw.ball.pos.x<FLEFT+10*cm)
			{
				assist_pos1.x=FLEFT+Rob_adge;
				assist_pos2.x=GoalL-Rob_rad40;
			}
			else if(zw.ball.pos.x<GoalL+Rob_adge)
			{
				assist_pos1.x=GoalL;
				assist_pos2.x=GoalL+2*cm;
			}
			else
			{
				assist_pos1.x=GoalL+Rob_adge;
				assist_pos2.x=GoalL+Rob_adge;
			}

			if(zw.ball.dy<-0.5*cm && zw.ball.pos.x>PenalB)
			{
				assist_pos1.y=PenalB+5*cm;
				assist_pos2.y=PenalT;
			}
			else if(zw.ball.dy>0.5*cm && zw.ball.pos.x<PenalT)
			{
				assist_pos1.y=PenalT-5*cm;
				assist_pos2.y=PenalB;
			}
			else
			{			
				if(zw.ball.pos.y<FMIDY)
				{
					assist_pos1.y=PenalT-5*cm;
					assist_pos2.y=PenalB;
				}
				else
				{
					assist_pos1.y=PenalB+5*cm;
					assist_pos2.y=PenalT;
				}
			}
		}	

		else//if(zw.ball.pos.x>=PenalL)
		{
			if(zw.ball.pos.x<PenalR-Rob_adge 
				|| zw.ball.pos.y<PenalB-Rob_adge || zw.ball.pos.y>PenalT+Rob_adge)
			{
				double dpgx=zw.ball.pos.x-FRIGHT;
				double dpgy=zw.ball.pos.y-FMIDY;
				double dist_pg=sqrt(dpgx*dpgx+dpgy*dpgy);

				assist_pos2.x=zw.ball.pos.x-30*cm*dpgx/dist_pg;
				assist_pos2.y=zw.ball.pos.y-30*cm*dpgy/dist_pg;

				if(assist_pos2.x>PenalR-Rob_adge 
					&& assist_pos2.y>PenalB-Rob_adge && assist_pos2.y<PenalT+Rob_adge)
				{
					if(fabs(dpgy/dpgx)>8.0/7.0)