strategy.cpp

足球机器人比赛程序

	}

	if ( Ay < 27 )
	{
		velocityLeft = 100;
		velocityRight = 100;
	}

	if ( Ay > 43 )
	{
		velocityLeft = -100;
		velocityRight = -100;
	}

	double Tr = robot->rotation;
	if ( Tr < 0.001 )
		Tr = Tr + 360;
	if ( Tr > 360.001 )
		Tr = Tr - 360;
	if ( Tr > 270.5 )
		velocityRight = velocityRight + fabs ( Tr - 270 );//调整角度
	else if ( Tr < 269.5 )
		velocityLeft = velocityLeft + fabs ( Tr - 270 );

	robot->velocityLeft = velocityLeft;
	robot->velocityRight = velocityRight;
}

void Attack2 ( Robot *robot, Environment *env )//自带射门函数,踢不到球
{
	Vector3D t = env->currentBall.pos;
	double r = robot->rotation;
	if ( r < 0 ) r += 360;
	if ( r > 360 ) r -= 360;
	double vl = 0, vr = 0;

	if ( t.y > env->fieldBounds.top - 2.5 ) t.y = env->fieldBounds.top - 2.5;//边角调整
	if ( t.y < env->fieldBounds.bottom + 2.5 ) t.y = env->fieldBounds.bottom + 2.5;
	if ( t.x > env->fieldBounds.right - 3 ) t.x = env->fieldBounds.right - 3;
	if ( t.x < env->fieldBounds.left + 3 ) t.x = env->fieldBounds.left + 3;

	double dx = robot->pos.x - t.x;
	double dy = robot->pos.y - t.y;

	double dxAdjusted = dx;
	double angleToPoint = 0;

	if ( fabs ( robot->pos.y - t.y ) > 7 || t.x > robot->pos.x )
		dxAdjusted -= 5;

	if ( dxAdjusted == 0 )
	{
		if ( dy > 0 )
			angleToPoint = 270;
		else
			angleToPoint = 90;
	}
	else if ( dy == 0 )
	{
		if ( dxAdjusted > 0 )
			angleToPoint = 360;
		else
			angleToPoint = 180;
	}
	else
		angleToPoint = atan ( fabs ( dy / dx ) ) * 180.0 / PI;

	if ( dxAdjusted > 0 )
	{
		if ( dy > 0 )
			angleToPoint -= 180;
		else if ( dy < 0 )
			angleToPoint = 180 - angleToPoint;
	}
	if ( dxAdjusted < 0 )
	{
		if ( dy > 0 )
			angleToPoint = - angleToPoint;
		else if ( dy < 0 )
			angleToPoint = 90 - angleToPoint;
	}

	if ( angleToPoint < 0 ) angleToPoint = angleToPoint + 360;
	if ( angleToPoint > 360 ) angleToPoint = angleToPoint - 360;
	if ( angleToPoint > 360 ) angleToPoint = angleToPoint - 360;

	double c = r;

	double angleDiff = fabs ( r - angleToPoint );

	if ( angleDiff < 40 )
	{
		vl = 100;
		vr = 100;
		if ( c > angleToPoint )
			vl -= 10;
		if ( c < angleToPoint )
			vr -= 10;
	}
	else
	{
		if ( r > angleToPoint )
		{
			if ( angleDiff > 180 )
				vl += 360 - angleDiff;
			else
				vr += angleDiff;
		}
		if ( r < angleToPoint )
		{
			if ( angleDiff > 180 )
				vr += 360 - angleDiff;
			else
				vl += angleDiff;
		}
	}
	NearBound2 ( robot, vl, vr, env );
}

void NearBound2 ( Robot *robot, double vl, double vr, Environment *env )//机器人接近边缘,减速
{
	//Vector3D t = env->currentBall.pos;
	Vector3D a = robot->pos;
	double r = robot->rotation;

	if ( a.y > env->fieldBounds.top - 15 && r > 45 && r < 130 )
	{
		if ( vl > 0 )
			vl /= 3;
		if ( vr > 0 )
			vr /= 3;
	}

	if ( a.y < env->fieldBounds.bottom + 15 && r < -45 && r > -130 )
	{
		if ( vl > 0 ) vl /= 3;
		if ( vr > 0 ) vr /= 3;
	}

	if ( a.x > env->fieldBounds.right - 10 )
	{
		if ( vl > 0 )
			vl /= 2;
		if ( vr > 0 )
			vr /= 2;
	}

	if ( a.x < env->fieldBounds.left + 10 )
	{
		if ( vl > 0 )
			vl /= 2;
		if ( vr > 0 )
			vr /= 2;
	}
	robot->velocityLeft = vl;
	robot->velocityRight = vr;
}

void Defend ( Robot *robot, Environment *env, double low, double high )//原始防守策略,只能以270度方向做直线运动
{
	double vl = 0, vr = 0;
	Vector3D z = env->currentBall.pos;

	double Tx = env->goalBounds.right - z.x;
	double Ty = env->fieldBounds.top - z.y;
	Vector3D a = robot->pos;
	a.x = env->goalBounds.right - a.x;
	a.y = env->fieldBounds.top - a.y;

	if ( a.y > Ty + 0.9 && a.y > low )//40
	{
		vl = -100;
		vr = -100;
	}
	if ( a.y < Ty - 0.9 && a.y < high )//70
	{
		vl = 100;
		vr = 100;
	}
	if ( a.y < low )
	{
		vl = 100;
		vr = 100;
	}
	if ( a.y > high )
	{
		vl = -100;
		vr = -100;
	}

	double Tr = robot->rotation;

	if ( Tr < 0.001 )
		Tr += 360;
	if ( Tr > 360.001 )
		Tr -= 360;
	if ( Tr > 270.5 )
		vr += fabs ( Tr - 270 );//调整角度
	else if ( Tr < 269.5 )
		vl += fabs ( Tr - 270 );

	NearBound2 ( robot, vl ,vr, env );
}

void GoalKeeper_In_Left(Robot *robot,Environment *env)
{
	double	estimate_x=FLEFTX+0.324;
	double	estimate_y=env->currentBall.pos.y;
	double	enter_y=env->currentBall.pos.y;
	
	if(env->currentBall.pos.x>=FLEFTX+7)//预测,对estimate_y作出修正
	{
		if(env->currentBall.pos.x<env->lastBall.pos.x)
		{
			double k=(env->currentBall.pos.y-env->lastBall.pos.y)/(env->currentBall.pos.x-env->lastBall.pos.x);
			enter_y=k*(estimate_x-env->currentBall.pos.x)+env->currentBall.pos.y;
		}
		if(enter_y>33.9320 && enter_y<49.6801)
		{
			estimate_y=enter_y;
		}
	}
	
	if(estimate_y>49.6801-1.85)
		estimate_y=49.6801-1.85;
	else if(estimate_y<33.9320+1.85)
		estimate_y=33.9320+1.85;

	if(GoaliePosition(robot,estimate_x,estimate_y))
	{
		if(sqrt((robot->pos.x-env->currentBall.pos.x)*(robot->pos.x-env->currentBall.pos.x)
			+(robot->pos.y-env->currentBall.pos.y)*(robot->pos.y-env->currentBall.pos.y))<=4)
			{
				if(env->currentBall.pos.y>robot->pos.y)
					Velocity(robot,-125,125);
				else
					Velocity(robot,125,-125);
			}
		else
			Angle1(robot,90);
	}
}

void GoalKeeper_In_Right(Robot *robot,Environment *env)//蓝队(右队)守门员在里面
{
	double	estimate_x=FRIGHTX-0.324;//93.75;
	double	estimate_y=env->currentBall.pos.y;
	double	enter_y=env->currentBall.pos.y;

	if(env->currentBall.pos.x<=FRIGHTX-7)//预测,对estimate_y作出修正
	{
		if(env->currentBall.pos.x>env->lastBall.pos.x)
		{
			double k=(env->currentBall.pos.y-env->lastBall.pos.y)/(env->currentBall.pos.x-env->lastBall.pos.x);
			enter_y=k*(estimate_x-env->currentBall.pos.x)+env->currentBall.pos.y;
		}
		if(enter_y>33.9320 && enter_y<49.6801)
		{
			estimate_y=enter_y;
		}
	}

	if(estimate_y>49.6801-1.85)
		estimate_y=49.6801-1.85;
	else if(estimate_y<33.9320+1.85)
		estimate_y=33.9320+1.85;
	
	if(GoaliePosition(robot,estimate_x,estimate_y))
	{
		if(sqrt((robot->pos.x-env->currentBall.pos.x)*(robot->pos.x-env->currentBall.pos.x)
			+(robot->pos.y-env->currentBall.pos.y)*(robot->pos.y-env->currentBall.pos.y))<=4)
			{
				if(env->currentBall.pos.y>robot->pos.y)
					Velocity(robot,125,-125);
				else
					Velocity(robot,-125,+125);
			}
		else
			Angle1(robot,90);
	}
}

void GoalKeeper_Out(Robot *robot,Environment *env)//蓝队(右队)守门员在外面,有出击动作
{
	double estimate_x=85;//90.4616;//守门员的原始x坐标
	double estimate_y=env->currentBall.pos.y;
	double enter_y=0, dx, dy,d_e;

	if(env->currentBall.pos.x<=45)
	{		
		double angle_up_PI=atan2((double)(49.6801-env->currentBall.pos.y),(double)(93.4259-env->currentBall.pos.x));
		double angle_bottom_PI=atan2((double)(33.9320-env->currentBall.pos.y),(double)(93.4259-env->currentBall.pos.x));
		estimate_y=env->currentBall.pos.y+(estimate_x-env->currentBall.pos.x)*tan((angle_up_PI+angle_bottom_PI)*0.5);

		dx=estimate_x-robot->pos.x;
		dy=estimate_y-robot->pos.y;
		d_e=sqrt(dy*dy+dx*dx);
		
		if(GoaliePosition(robot,estimate_x,estimate_y))
		{
			Angle1(robot,90);
		}
	}
	
	else
	{
		if(env->currentBall.pos.y>49.6801+5 && env->currentBall.pos.y<49.6801+10 && env->currentBall.pos.x>88 && env->currentBall.pos.x<93.4259)
		{
			if(GoaliePosition(robot,env->currentBall.pos.x,env->currentBall.pos.y))
			{
				Angle1(robot,90);
			}
		}

		else if(env->currentBall.pos.y>33.9320-10 && env->currentBall.pos.y<33.9320-5 && env->currentBall.pos.x>88 && env->currentBall.pos.x<93.4259) 
		{
			if(GoaliePosition(robot,env->currentBall.pos.x,env->currentBall.pos.y))
			{
				Angle1(robot,90);
			}
		}
		
		else
		{			
			if(env->currentBall.pos.x>env->lastBall.pos.x)
			{
				double k=(env->currentBall.pos.y-env->lastBall.pos.y)/(env->currentBall.pos.x-env->lastBall.pos.x);
				enter_y=k*(estimate_x-env->currentBall.pos.x)+env->currentBall.pos.y;
			}

			if(enter_y>33.9320 && enter_y<49.6801)
			{
				estimate_y=enter_y;
			}

			if(estimate_y>49.6801)
				estimate_y=49.6801;
			else if(estimate_y<33.9320)
				estimate_y=33.9320;
			if(GoaliePosition(robot, estimate_x, estimate_y))
			{
				Angle1(robot,90);
			}
		}			
	}
}

void AKick_Left(Robot *robot,Environment *env)
{
	double	e_theta,x,y, d_x, d_y, d_angle=0.0;
	double	dev;
	int		vr,vl;
	double	dest, V_c;
	double	n_field, p_field,r_angle;
	static double mball_x = 0.0;
	static double mball_y = 0.0;

	mball_x = env->currentBall.pos.x;
	mball_y = env->currentBall.pos.y;
	

	x = robot->pos.x-mball_x;
	y = robot->pos.y-mball_y;

	if(env->currentBall.pos.y>33.9320 && env->currentBall.pos.y<49.6801)  //if(PositionOfBall[1]>45. && PositionOfBall[1]<85.)//球在球门线以内
		dest = 0.;                    
	else {//球在球门线以外
		d_x = (double)(93.4259 - env->currentBall.pos.x);  //d_x = (double)(150. - PositionOfBall[0]) ;
		d_y = (double)(41.80605 - env->currentBall.pos.y);  //d_y = (double)(65. - PositionOfBall[1]);
	
		if(d_x == 0.)   //球的x坐标为150，即在边界
		{
			if(d_y>0)
				dest = 90.;
			else
				dest = 270.;
		}
		else
			dest = 180.0/PI*atan2((double)(d_y), (double)(d_x));
	}

	if(env->currentBall.pos.y>77.2392-5)  //if(PositionOfBall[1]>120. )//球在上边界
		if( env->currentBall.pos.x>robot->pos.x )  //if( PositionOfBall[0]>PositionOfHomeRobot[whichrobot][0] )	
			dest = 0.;	
		else
			dest = 10.;
		
	if(env->currentBall.pos.y<6.3730+5)  //if(PositionOfBall[1]<10. )//球在下边界
		if( env->currentBall.pos.x>robot->pos.x )  //if( PositionOfBall[0]>PositionOfHomeRobot[whichrobot][0] ) 
			dest = 0.;
		else
			dest = 350.;

	n_field = Field(x,y,dest);      


	r_angle = robot->rotation;//机器人角度

	e_theta = n_field - r_angle;

	while(e_theta > 180.)	
		e_theta -= 360.;
	while(e_theta <-180.)	
		e_theta += 360.;
	
	V_c=80; //V_c = 40;


	double Ka;
	if (e_theta > -75 && e_theta < 75) 
	{			
			vr=vl=(int)V_c;
			if ( vl > 50 ) vl = 50;
			if ( vr > 50 ) vr = 50;
			if ( vl < 20 ) vl = 20;
			if ( vr < 20 ) vr = 20;

			if ( vl < 30 || ( e_theta >= -20 && e_theta <=20 ) ) 
			{
				if ( vl < 20 || ( e_theta >= -35 && e_theta <=35 ) ) 
				{
					Ka=0.6;  //0.15;
					if( e_theta >= -5 && e_theta <=5 )
					{
					vr=125;  //60;
					vl=125;  //60;
					}
				}
				else {
					vl = 80;  //18;
					vr = 80;  //18;
					Ka = 0.5; //0.12;	
				}								
			}
			else 
			{
				vl = 100;  //25;
				vr = 100;  //25;
				Ka = 0.5;  //0.15;
			}
			vl = (int)(vl - (Ka*e_theta));
			vr = (int)(vr + (Ka*e_theta));
	}

	else
	{
		vl = (int)(-0.4*e_theta);  //vl = (int)(-0.12*e_theta);
		vr = (int)(0.4*e_theta);  //vr = (int)(0.12*e_theta);
	}


	Velocity(robot, (int)vl, (int)vr);

	//tmp_angle[whichrobot]=(int)e_theta;

   	if(env->currentBall.pos.x>93.4259 || env->currentBall.pos.x<6.8118) //if(PositionOfBall[0] > 150.0 || PositionOfBall[0] < 0.0)
   		Velocity(robot,0,0);
}

#define G 		    3  // 5//4.
#define D 			1  // 2//3.
#define A 			1  // 1.5//1. 1.5 is good.
#define K_FIELD 	2.6//2.5
double Field(double x,double y,double dest)
{
	double out;
	double a_theta, b_theta;
	double dx,dy,tx,ty;
	int flag = 0;
	

	tx = x;	ty = y;
	x =  tx*cos(PI/180.*dest) + ty*sin(PI/180.*dest);
	y = -tx*sin(PI/180.*dest) + ty*cos(PI/180.*dest);
 
	if( y<0 )
	{
		 flag = 1;
		 y = -y;
	}
 


	if( y<G ){
		if( x<0. )
		{
			if(flag== 0)
			{
				if(fabs(x) > 8.0)
					out =-15.+ dest;
				else
					out = -15. - 13. + dest;
			}
			else
			{		
				if(fabs(x) > 8.0)
					out = 15.+ dest;
				else
					out = 1