action.cpp

FIRA机器人足球比赛5V5仿真组的一般的防守策略

#include "strategy.h"
#include "vsv.h"
//
/* RegulateAngle 规范angle的大小在（-180，+180）之间
*/
//
void RegulateAngle(double &angle)
{
	while(angle >= 180.0 )angle-=360.0;
	while(angle < -180.0 )angle+=360.0;
}

//
/*toint 转换成整数 （四舍五入）
*/
//
int toint(double num)
 {
 	return num*10 < (int)num*10+5?(int)num:(int)num+1;
}

//
/*Atan 求y/x正切值对应的角度（-180，+180）之间
*/
//

double Atan(double y,double x)
{
	if(x != 0.0 || y != 0.0) 
	return 180*atan2(y,x)/PI;
	else return 0.0;
}

//
/*Atan (end-begin)矢量的角度，（-180，+180）之间
*/
//
double Atan(Vector3D begin,Vector3D end)
{
	double y,x;
	y=end.y - begin.y ;
	x=end.x - begin.x ;
	return Atan(y,x);
}

//
/*Distance  两点之间距离
*/
//
double Distance (Vector3D pos1,Vector3D pos2)
{
	return sqrt( (pos1.x-pos2.x)*(pos1.x-pos2.x) + (pos1.y-pos2.y)*(pos1.y-pos2.y) );
}

/*
DllMain,Create,Destroy这三个函数建议大家不用理会，
只是如果你在策略之间如果有数据需要赋初值可以在Create 中进行
*/
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;
}

extern "C" STRATEGY_API void Create ( Environment *env )
{
	//初始化数据
	env->userData=(void* ) new Mydata;

	Mydata* p;
	p=(Mydata*)env->userData;//???
	
	p->n=125;			//these two veriaty is for the test funtion!
	p->B_N=true;		//


	p->debug = true;				//是 调试
	
	p->time[1]=0;					//初始化时间
	p->time[0]=125;					//初始化时间

	p->bgoalball=0;					//初始化非求门球
	p->nfreeball=0;					//初始化非求门球
	p->nplaceball=0;

	p->mainrobot = -1;
	p->cutrobot = -1;
	p->slowrobot = -1;
	p->chooserobot = 0;

	for(int i =0 ;i < 5;i++)
	{
		p->robot[i].pos.x=InlitializeMyPosition_X[i];
		p->robot[i].pos.y=InlitializeMyPosition_Y[i];
		p->robot[i].pos.z=InlitializeMyPosition_Z[i];			//what is 'z'?

		p->myoldpos[i].x=InlitializeMyPosition_X[i];
		p->myoldpos[i].y=InlitializeMyPosition_Y[i];
		p->myoldpos[i].z=InlitializeMyPosition_Z[i];			//this z is the direction that the robot faces

		p->myspeed[i].x=0;
		p->myspeed[i].y=0;
		p->myspeed[i].z=0;

		p->myoldvelocity[i].x=0;
		p->myoldvelocity[i].y=0;
		p->myoldvelocity[i].z=0;

		p->opp[i].pos.x = InlitializeOppPosition_X[i];			//对方坐标
		p->opp[i].pos.y = InlitializeOppPosition_Y[i];			//对方坐标
		p->opp[i].pos.z = InlitializeOppPosition_Z[i];			//对方坐标

		p->opoldpos[i].x = InlitializeOppPosition_X[i];			//对方 旧坐标
		p->opoldpos[i].y = InlitializeOppPosition_Y[i];			//对方 旧坐标
		p->opoldpos[i].z = InlitializeOppPosition_Z[i];			//对方 旧坐标

		p->opspeed[0].x = 0;			//对方 队员速度
		p->opspeed[0].y = 0;			//对方 队员速度
		p->opspeed[0].z = 0;			//对方 队员速度

	}

	p->locked=false;				// 判断了 场地 ??
	p->mygrand=true;				// 是 黄队 ??

	p->oldball.x = (FLEFT + FRIGHT) / 2.0;
	p->oldball.y = (FTOP + FBOT) / 2.0;
	p->oldball.z = 0;							//what is 'z' mean? the ball's speed ??

	p->curball.x = (FLEFT + FRIGHT) / 2.0;
	p->curball.y = (FTOP + FBOT) / 2.0;
	p->curball.z = 0;

	p->preball.x = (FLEFT + FRIGHT) / 2.0;
	p->preball.y = (FTOP + FBOT) / 2.0;
	p->preball.z = 0;

	p->ballspeed.x =0;
	p->ballspeed.y =0;
	p->ballspeed.z =0;

	if(p->debug)
	{
		p->debugfile = fopen("c:\\strategy\\SCU_introduction.txt","w"); 
		if( !p->debugfile )
			p->debugfile = fopen("c:\\strategy\\SCU_introduction.doc","w");
		if( !p->debugfile )
			p->debug = false;
//		fclose(p->debugfile); 
	}

}

extern "C" STRATEGY_API void Destroy ( Environment *env )
{
	Mydata * p;
	p=(Mydata *)env->userData;

//	if(p->debug)
		fclose(p->debugfile);
	
	if ( env->userData != NULL )	delete ( Mydata * ) env->userData;
}

/////////
/*Stategy  策略的入口，该函数的形式不能修改，否则会出错
参数 Environment *env 用来获取当前的比赛信息。
并且通过修改 Environment.home.velocityLeft, Environment.home.velocityRight的值来控制机器人的运动
具体工作由
	See(env);			// 预处理
	Action(env);		//策略
	End ( env );		//后期处理
	分担了
*/
////////
extern "C" STRATEGY_API void Strategy ( Environment *env )
//////////////适用于  !!蓝队 
{
	Mydata * p;
	p=(Mydata *)env->userData;

	if(!p->locked)		// 是 判断场地了 ??
	{//确定区域,blue or yellow
		if( env->home[0].pos.x < 50.0 )
			p->mygrand=true; /// 是 = 黄队??
		else
			p->mygrand=false;
		p->locked=true;
	}

	See(env);			// 预处理

	Action(env);		//策略
	

	End ( env );		//后期处理

}

//
/* See 预处理
1。进行坐标变换
2。求出一些策略中经常用到的量，如每个队员的速率和角速度等
*/
//
void See ( Environment *env )
{
	Mydata * p;
	p=(Mydata *)env->userData;

	int i=0;

	if(p->mygrand)
	{///场地变换，球坐标变换
		//我方是黄队

		p->gameState = env->gameState ;
		switch(env->whoseBall)
		{
		case 0 ://自由球,在处理球的时候这种方法可能不保险
			//最好包括用球的坐标，来判断谁的点球，和其他的球
			p->whoseBall = 0;
		case 1 :
			p->whoseBall = 1;
		case 2:
			p->whoseBall =2 ;
		}


		p->curball.x = env->currentBall.pos.x;		//球坐标变化
		p->curball.y = env->currentBall.pos.y;


		for(i=0;i<5;i++)
		{
			p->robot[i].pos.x = env->home[i].pos.x ;	//我方队员坐标变换
			p->robot[i].pos.y = env->home[i].pos.y ;
			p->robot[i].rotation= env->home[i].rotation;
			
			p->opp[i].pos.x =env->opponent[i].pos.x;	//对方坐标变换
			p->opp[i].pos.y =env->opponent[i].pos.y;
			p->opp[i].rotation =env->opponent[i].rotation;
			RegulateAngle(p->opp[i].rotation);

		}
	}
	else
	{//we are blue
		p->gameState = env->gameState ;
		switch(env->whoseBall)
		{
		case 0 ://自由球,在处理球的时候这种方法可能不保险
			//最好包括用球的坐标，来判断谁的点球，和其他的球
			p->whoseBall = 0;
		case 1 :
			p->whoseBall = 2;
		case 2:
			p->whoseBall = 1;
		}


		p->curball.x =FLEFT+FRIGHT + CORRECTX - env->currentBall.pos.x;		//球坐标变化
		p->curball.y =FBOT+FTOP + CORRECTY - env->currentBall.pos.y;
		//p->curball.z = env->currentBall.pos.z;

		for(i=0;i<5;i++)
		{
			p->robot[i].pos.x =FLEFT+FRIGHT + CORRECTX - env->home[i].pos.x ;	//我方队员坐标变换
			p->robot[i].pos.y =FBOT+FTOP + CORRECTY - env->home[i].pos.y ;
			//p->robot[i].pos.z = env->home[i].pos.z ;
			p->robot[i].rotation= 180.0 + env->home[i].rotation;
			RegulateAngle(p->robot[i].rotation);
			
			p->opp[i].pos.x = FLEFT+FRIGHT + CORRECTX- env->opponent[i].pos.x;	//对方坐标变换
			p->opp[i].pos.y = FBOT+FTOP + CORRECTY - env->opponent[i].pos.y;
			//p->opp[i].pos.z = env->opponent[i].pos.z;
			p->opp[i].rotation =  180 + env->opponent[i].rotation;
			RegulateAngle(p->opp[i].rotation);
		}
	}

////第一次处理速度  (上次)
	for(i=0;i<5;i++)
	{///speed
		p->myspeed[i].x = ( p->robot[i].pos.x - p->myoldpos[i].x);	//70为比例系数，有待调整
		p->myspeed[i].y = ( p->robot[i].pos.y - p->myoldpos[i].y);
		p->myspeed[i].z = Atan(p->myspeed[i].y,p->myspeed[i].x);		//关于他的 运动方向 和转角速度,考虑中

		p->opspeed[i].x = ( p->opp[i].pos.x - p->opoldpos[i].x);
		p->opspeed[i].y = ( p->opp[i].pos.y - p->opoldpos[i].y);
		p->opspeed[i].z = Atan(p->opspeed[i].y,p->opspeed[i].x);
	}

	p->ballspeed.x = p->curball.x - p->oldball.x;
	p->ballspeed.y = p->curball.y - p->oldball.y;
	p->ballspeed.z = Atan( p->ballspeed.y , p->ballspeed.x );
////以上部不可直接作为当前数据
///下面开始处理当前的真实数据
//////处理robot坐标
	double v,a,b,c,omiga,angle;
	for(i=0;i<5;i++)
	{

		omiga = p->robot[i].rotation - p->myoldpos[i].z ;
		RegulateAngle(omiga);
		omiga = AngleOne(omiga,p->myoldvelocity[i].x , p->myoldvelocity[i].y);
		c = p->robot[i].rotation;	
		p->robot[i].rotation+=omiga;
		RegulateAngle(p->robot[i].rotation);

		v = sqrt((p->myspeed[i].x * p->myspeed[i].x) + (p->myspeed[i].y * p->myspeed[i].y));
		angle = p->robot[i].rotation - p->myspeed[i].z;
		RegulateAngle(angle);
		if(angle >-90 && angle < 90 )		
			v=v;
		else
			v=-v;

		v=VelocityOne(v,p->myoldvelocity[i].x , p->myoldvelocity[i].y);
		a=p->robot[i].pos.x;
		b=p->robot[i].pos.y;

		p->robot[i].pos.x += v*cos( p->robot[i].rotation * PI / 180) ;
		p->robot[i].pos.y += v*sin( p->robot[i].rotation * PI / 180) ;
///处理撞墙
//不处理最好

////处理撞墙		
		p->myoldpos[i].x =a;
		p->myoldpos[i].y =b;
		p->myoldpos[i].z =c;

		p->myspeed[i].x = ( p->robot[i].pos.x - p->myoldpos[i].x );	//70为比例系数，有待调整
		p->myspeed[i].y = ( p->robot[i].pos.y - p->myoldpos[i].y );
		p->myspeed[i].z = Atan( p->myspeed[i].y , p->myspeed[i].x );
	}

/////////	 预测球的坐标

	double x,y;
	x = p->curball.x ;
	y = p->curball.y ;
	
	PredictBall(env);		//求到现在球的位置
	p->curball = p->preball;

	p->oldball.x = x;
	p->oldball.y = y;

	PredictBall(env);		//预测下一步球的位置


	p->ballspeed.x = p->curball.x - p->oldball.x;
	p->ballspeed.y = p->curball.y - p->oldball.y;
	p->ballspeed.z = Atan( p->ballspeed.y , p->ballspeed.x );
/////////	 预测球的坐标

	for(i=0;i<5;i++)		
	{//赋初值
		p->robot[i].velocityLeft = 0;
		p->robot[i].velocityRight = 0;
	}

	
	p->ballArea = CheckBall(env);

	///计时
	///很有用处的
	p->time[1]++;
	if(p->time[1]==50){
		p->time[1]=0;
		p->time[0]++;
	}
	if(p->curball.y>42){
	  p->mainrobot=3;
	  p->slowrobot=1;
	  p->cutrobot=4;
	  p->defentrobot=2;
	}
	else{
		p->mainrobot=4;
		p->slowrobot=2;
		p->cutrobot=3;
		p->defentrobot=1;
	}

	if(p->debug)
	{
		fprintf(p->debugfile, "%d,",p->time[1]);
	}

}

//
/* End 这个函数在策略执行完成以后被调用
1。提交对队员轮速的修改
2。保存本周期的状态供下次计算使用
*/
//

void End ( Environment *env )
{
	///做一些清扫的工作
	///做一些记录整理工作
	
	Mydata * p;
	p=(Mydata *)env->userData;
	
	int i=0;

	for(i=0;i<5;i++)
	{//速度
		env->home[i].velocityLeft = p->robot[i].velocityLeft;
		env->home[i].velocityRight = p->robot[i].velocityRight;
		
		p->myoldvelocity[i].x=p->robot[i].velocityLeft;
		p->myoldvelocity[i].y=p->robot[i].velocityRight;

	}

	if(p->mygrand)
	{///场地变换，球坐标变换
		p->oldball.x = env->currentBall.pos.x;		//球坐标变化
		p->oldball.y = env->currentBall.pos.y;
		//p->curball.z = env->currentBall.pos.z;


		for(i=0;i<5;i++)
		{
			p->myoldpos[i].x = env->home[i].pos.x ;	//我方队员坐标变换
			p->myoldpos[i].y = env->home[i].pos.y ;
			//p->robot[i].pos.z = env->home[i].pos.z ;
			p->myoldpos[i].z= env->home[i].rotation;
			
			p->opoldpos[i].x =env->opponent[i].pos.x;	//对方坐标变换
			p->opoldpos[i].y =env->opponent[i].pos.y;
			//p->opp[i].pos.z = env->opponent[i].pos.z;
			p->opoldpos[i].z =env->opponent[i].rotation;
			RegulateAngle(p->opoldpos[i].z);

		}
	}
	else
	{
		p->oldball.x =FLEFT+FRIGHT + CORRECTX - env->currentBall.pos.x;		//球坐标变化
		p->oldball.y =FBOT+FTOP + CORRECTY - env->currentBall.pos.y;
		//p->curball.z = env->currentBall.pos.z;

		for(i=0;i<5;i++)
		{
			p->myoldpos[i].x =FLEFT+FRIGHT + CORRECTX - env->home[i].pos.x ;	//我方队员坐标变换
			p->myoldpos[i].y =FBOT+FTOP + CORRECTY - env->home[i].pos.y ;
			//p->robot[i].pos.z = env->home[i].pos.z ;
			p->myoldpos[i].z = 180.0 + env->home[i].rotation;
			RegulateAngle(p->myoldpos[i].z);
			
			p->opoldpos[i].x = FLEFT+FRIGHT + CORRECTX- env->opponent[i].pos.x;	//对方坐标变换
			p->opoldpos[i].y = FBOT+FTOP + CORRECTY - env->opponent[i].pos.y;
			//p->opp[i].pos.z = env->opponent[i].pos.z;
			p->opoldpos[i].z =  180 + env->opponent[i].rotation;
			RegulateAngle(p->opoldpos[i].z);
		}
	}

	if(p->debug)
	{
		fprintf(p->debugfile, "\n");
	}

}

//
/*Velocity 修改robot的左右轮速vl,vr

*/
//
void Velocity(Environment *env, int robot,double vl,double vr)
{
	Mydata * p;
	p=(Mydata *)env->userData;

	//vl,vr都有取值范围的!!!
	if(vl>125)vl=125;
	if(vl<-125)vl=-125;
	if(vr>125)vr=125;
	if(vr<-125)vr=-125;
	if(vl==0 && vr!=0)
		{
		vl=0.00001;
		}
	if(vr==0 && vl!=0)
	{
		vr=0.00001;
	}
	p->robot[robot].velocityLeft = vl;
	p->robot[robot].velocityRight= vr;


}

//
/*Angle 让robot转到angle的方向

*/
//
void Angle( Environment *env, int robot,double angle)
{

	Mydata * p;
	p=(Mydata *)env->userData;

	double speed = 0;		//和pangle接轨
	double accuracy=1;
	double turnangle=0,nextangle=0;
	double FF=125;		//最大减速度

	turnangle = angle -p->robot[robot].rotation;
	RegulateAngle(turnangle);
if(turnangle < 1 && turnangle >-1)
{
	Velocity(env,robot,0,0);
	return ;
}
else if(turnangle < 2 && turnangle >-2)
	FF=10;
else if( turnangle >-3 && turnangle < 3)
	FF=15;
else if( turnangle >-5 && turnangle < 5)
	FF=30;

	double v=p->robot[robot].rotation - p->myoldpos[robot].z ;
	RegulateAngle(v);
	double v1=v;
	double f=0;	//相当于减速时,右轮速度，
//	int n=0;
	bool turnleft=true;			//判断小车是否是该向左转
	double a=ANGLE_A;
	double b=ANGLE_B;

	if(turnangle > 90)
	{
		turnleft=false;
		turnangle-=180;
	}
	else if(turnangle >0)
	{
		turnleft=true;	
	}
	else if(turnangle > -90)
	{
		turnleft=false;	
	}
	else 
	{
		turnleft=true;
		turnangle+=180;		
	}

	if(turnleft)
	{//
		f=-FF;