strategy.cpp

足球机器人比赛程序

				Shoot1_Left(robot1,env);
				GoaliePosition(robot2,env->currentBall.pos.x+3,env->currentBall.pos.y+20);
				GoaliePosition(robot3,env->currentBall.pos.x-3,env->currentBall.pos.y+10);
			}
			if(leng2<leng1 && leng2<=leng3)
			{
				Shoot1_Left(robot2,env);
				GoaliePosition(robot1,env->currentBall.pos.x+3,env->currentBall.pos.y+20);
				GoaliePosition(robot3,env->currentBall.pos.x-3,env->currentBall.pos.y+10);
			}
			if(leng3<leng1 && leng3<leng2)
			{
				Shoot1_Left(robot3,env);
				GoaliePosition(robot1,env->currentBall.pos.x+3,env->currentBall.pos.y+20);
				GoaliePosition(robot2,env->currentBall.pos.x-3,env->currentBall.pos.y+10);
			}
		}
	}

	else if(env->currentBall.pos.x>=FRIGHTX-6)
	{
		if(leng1<=leng2 && leng1<=leng3)
		{
			Shoot1_Left(robot1,env);
			if(env->currentBall.pos.y>(FTOP+FBOT)/2)
			{
				if(GoaliePosition(robot2,env->currentBall.pos.x-8,GTOPY-2))
					AngleOfPosition(robot2,env->currentBall.pos.x,env->currentBall.pos.y);
				if(GoaliePosition(robot3,env->currentBall.pos.x-18,GBOTY-3))
					AngleOfPosition(robot3,env->currentBall.pos.x,env->currentBall.pos.y);
			}
			else
			{
				if(GoaliePosition(robot2,env->currentBall.pos.x-8,GBOTY+2))
					AngleOfPosition(robot2,env->currentBall.pos.x,env->currentBall.pos.y);
				if(GoaliePosition(robot3,env->currentBall.pos.x-18,GTOPY+3))
					AngleOfPosition(robot3,env->currentBall.pos.x,env->currentBall.pos.y);
			}
			
		}
		if(leng2<leng1 && leng2<=leng3)
		{
			Shoot1_Left(robot2,env);
			if(env->currentBall.pos.y>(FTOP+FBOT)/2)
			{
				if(GoaliePosition(robot1,env->currentBall.pos.x-8,GTOPY-2))
					AngleOfPosition(robot2,env->currentBall.pos.x,env->currentBall.pos.y);
				if(GoaliePosition(robot3,env->currentBall.pos.x-18,GBOTY-3))
					AngleOfPosition(robot3,env->currentBall.pos.x,env->currentBall.pos.y);
			}
			else
			{
				if(GoaliePosition(robot1,env->currentBall.pos.x-8,GBOTY+2))
					AngleOfPosition(robot2,env->currentBall.pos.x,env->currentBall.pos.y);
				if(GoaliePosition(robot3,env->currentBall.pos.x-18,GTOPY+3))
					AngleOfPosition(robot3,env->currentBall.pos.x,env->currentBall.pos.y);
			}
		}
		if(leng3<leng1 && leng3<leng2)
		{
			Shoot1_Left(robot3,env);
			if(env->currentBall.pos.y>(FTOP+FBOT)/2)
			{
				if(GoaliePosition(robot1,env->currentBall.pos.x-8,GTOPY-2))
					AngleOfPosition(robot1,env->currentBall.pos.x,env->currentBall.pos.y);
				if(GoaliePosition(robot2,env->currentBall.pos.x-18,GBOTY-3))
					AngleOfPosition(robot2,env->currentBall.pos.x,env->currentBall.pos.y);
			}
			else
			{
				if(GoaliePosition(robot1,env->currentBall.pos.x-8,GBOTY+2))
					AngleOfPosition(robot1,env->currentBall.pos.x,env->currentBall.pos.y);
				if(GoaliePosition(robot2,env->currentBall.pos.x-18,GTOPY+3))
					AngleOfPosition(robot2,env->currentBall.pos.x,env->currentBall.pos.y);
			}
		}		
	}

	else
	{
		if(leng1<=leng2 && leng1<=leng3)
		{
			Defence1_Left(robot1,env);
			GoaliePosition(robot2,robot1->pos.x+3,robot1->pos.y-10);
			GoaliePosition(robot3,robot1->pos.x,robot1->pos.y-20);
		}
		if(leng2<leng1 && leng2<=leng3)
		{
			Defence1_Left(robot2,env);
			GoaliePosition(robot1,robot2->pos.x,robot2->pos.y-10);
			GoaliePosition(robot3,robot2->pos.x,robot2->pos.y+10);
		}
		if(leng3<leng1 && leng3<leng2)
		{
			Defence1_Left(robot3,env);
			GoaliePosition(robot1,robot3->pos.x,robot3->pos.y+20);
			GoaliePosition(robot2,robot3->pos.x+3,robot3->pos.y+10);
		}
	}
}

void Attack_Right(Environment *env)//根据距离判断进攻队员
{//robot1,robot2,robot3,robot4的具体角色根据场上具体情况分配
	Robot *robot1, *robot2, *robot3, *robot4;
	int i,flag1,flag2,flag3,flag4;
    double leng[5],Y[5],MIN;
	leng[0]=0;
	Y[0]=0;
    for(i=1;i<5;i++)
	{
		leng[i]=leng(86,(FBOT+FTOP)/2,env->home[i].pos.x,env->home[i].pos.y);
        Y[i]=env->home[i].pos.y;
	}

    MIN=leng[1];
	flag4=1;
	for(i=2;i<5;i++)
	{
		if(leng[i]<MIN)
		{
			MIN=leng[i];
			flag4=i;
		}
	}
	robot4=&env->home[flag4];
    
    if(flag4==1) {flag1=2;flag2=3;flag3=4;}
	if(flag4==2) {flag1=1;flag2=3;flag3=4;}
	if(flag4==3) {flag1=1;flag2=2;flag3=4;}
	if(flag4==4) {flag1=1;flag2=2;flag3=3;}
    robot1=&env->home[flag1];
	robot2=&env->home[flag2];
	robot3=&env->home[flag3];
	
	if(env->currentBall.pos.x>=(FRIGHTX+FLEFTX)/2 && env->currentBall.pos.x<=86
		&& robot1->pos.x<env->currentBall.pos.x
		&& robot2->pos.x<env->currentBall.pos.x 
		&& robot3->pos.x<env->currentBall.pos.x)
	{
		Position1(robot4,env->currentBall.pos.x,env->currentBall.pos.y);
	}
	else
	{
		double estimate_x=85;
		double estimate_y=env->currentBall.pos.y;
		double enter_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-10 && enter_y<49.6801+10)
		{
			estimate_y=enter_y;
		}
		if(estimate_y>49.6801+10)
			estimate_y=49.6801+10;
		else if(estimate_y<33.9320-10)
			estimate_y=33.9320-10;
		
		if(GoaliePosition(robot4, estimate_x, estimate_y))
		{
			AngleOfPosition(robot4,env->currentBall.pos.x,env->currentBall.pos.y);
		}
	}
	/*
	if(Y[flag1]<=Y[flag2] && Y[flag1]<=Y[flag3])
	{
		robot1=&env->home[flag1];
		if(Y[flag2]<=Y[flag3])
		{
			robot2=&env->home[flag2];
			robot3=&env->home[flag3];
		}
		else
		{
			robot2=&env->home[flag3];
			robot3=&env->home[flag2];
		}
	}
	if(Y[flag2]<Y[flag1] && Y[flag2]<=Y[flag3])
	{
		robot1=&env->home[flag2];
		if(Y[flag1]<=Y[flag3])
		{
			robot2=&env->home[flag1];
			robot3=&env->home[flag3];
		}
		else
		{
			robot2=&env->home[flag3];
			robot3=&env->home[flag1];
		}
	}
	if(Y[flag3]<Y[flag1] && Y[flag3]<Y[flag2])
	{
		robot1=&env->home[3];
		if(Y[flag1]<=Y[flag2])
		{
			robot2=&env->home[flag1];
			robot3=&env->home[flag2];
		}
		else
		{
			robot2=&env->home[flag2];
			robot3=&env->home[flag1];
		}
	}
    */
	double leng1=leng(env->currentBall.pos.x,env->currentBall.pos.y,robot1->pos.x,robot1->pos.y);
	double leng2=leng(env->currentBall.pos.x,env->currentBall.pos.y,robot2->pos.x,robot2->pos.y);
	double leng3=leng(env->currentBall.pos.x,env->currentBall.pos.y,robot3->pos.x,robot3->pos.y);
    
	if(env->currentBall.pos.x>FLEFTX+5 && env->currentBall.pos.x<FRIGHTX-5)
	{
		if(env->currentBall.pos.y>FBOT+5 && env->currentBall.pos.y<FTOP-5)
		{
			if(leng1<=leng2 && leng1<=leng3)
			{
				Shoot1_Right(robot1,env);
				GoaliePosition(robot2,env->currentBall.pos.x+5,env->currentBall.pos.y-10);
				GoaliePosition(robot3,env->currentBall.pos.x,env->currentBall.pos.y-20);
			}
			if(leng2<leng1 && leng2<=leng3)
			{
				Shoot1_Right(robot2,env);
				GoaliePosition(robot1,env->currentBall.pos.x-5,env->currentBall.pos.y-10);
				GoaliePosition(robot3,env->currentBall.pos.x+3,env->currentBall.pos.y+10);
			}
			if(leng3<leng1 && leng3<leng2)
			{
				Shoot1_Right(robot3,env);
				GoaliePosition(robot1,env->currentBall.pos.x+3,env->currentBall.pos.y+20);
				GoaliePosition(robot2,env->currentBall.pos.x-5,env->currentBall.pos.y+10);
			}
		}
		
		else if(env->currentBall.pos.y>=FTOP-5)
		{
			if(leng1<=leng2 && leng1<=leng3)
			{
				Shoot1_Right(robot1,env);
				GoaliePosition(robot2,env->currentBall.pos.x+5,env->currentBall.pos.y-10);
				GoaliePosition(robot3,env->currentBall.pos.x-2,env->currentBall.pos.y-20);
			}
			if(leng2<leng1 && leng2<=leng3)
			{
				Shoot1_Right(robot2,env);
				GoaliePosition(robot1,env->currentBall.pos.x-2,env->currentBall.pos.y-10);
				GoaliePosition(robot3,env->currentBall.pos.x-5,env->currentBall.pos.y-20);
			}
			if(leng3<leng1 && leng3<leng2)
			{
				Shoot1_Right(robot3,env);
				GoaliePosition(robot1,env->currentBall.pos.x+4,env->currentBall.pos.y-10);
				GoaliePosition(robot2,env->currentBall.pos.x-3,env->currentBall.pos.y-20);
			}
		}
		
		else
		{
			if(leng1<=leng2 && leng1<=leng3)
			{
				Shoot1_Right(robot1,env);
				GoaliePosition(robot2,env->currentBall.pos.x-3,env->currentBall.pos.y+20);
				GoaliePosition(robot3,env->currentBall.pos.x+3,env->currentBall.pos.y+10);
			}
			if(leng2<leng1 && leng2<=leng3)
			{
				Shoot1_Right(robot2,env);
				GoaliePosition(robot1,env->currentBall.pos.x-3,env->currentBall.pos.y+20);
				GoaliePosition(robot3,env->currentBall.pos.x+3,env->currentBall.pos.y+10);
			}
			if(leng3<leng1 && leng3<leng2)
			{
				Shoot1_Right(robot3,env);
				GoaliePosition(robot1,env->currentBall.pos.x-3,env->currentBall.pos.y+20);
				GoaliePosition(robot2,env->currentBall.pos.x+3,env->currentBall.pos.y+10);
			}
		}
	}

	else if(env->currentBall.pos.x<=FLEFTX+6)
	{
		if(leng1<=leng2 && leng1<=leng3)
		{
			Shoot1_Right(robot1,env);
			if(env->currentBall.pos.y>(FTOP+FBOT)/2)
			{
				if(GoaliePosition(robot2,env->currentBall.pos.x+8,GTOPY-2))
					AngleOfPosition(robot2,env->currentBall.pos.x,env->currentBall.pos.y);
				if(GoaliePosition(robot3,env->currentBall.pos.x+18,GBOTY-3))
					AngleOfPosition(robot3,env->currentBall.pos.x,env->currentBall.pos.y);
			}
			else
			{
				if(GoaliePosition(robot2,env->currentBall.pos.x+8,GBOTY+2))
					AngleOfPosition(robot2,env->currentBall.pos.x,env->currentBall.pos.y);
				if(GoaliePosition(robot3,env->currentBall.pos.x+18,GTOPY+3))
					AngleOfPosition(robot3,env->currentBall.pos.x,env->currentBall.pos.y);
			}
			
		}
		if(leng2<leng1 && leng2<=leng3)
		{
			Shoot1_Right(robot2,env);
			if(env->currentBall.pos.y>(FTOP+FBOT)/2)
			{
				if(GoaliePosition(robot1,env->currentBall.pos.x+8,GTOPY-2))
					AngleOfPosition(robot2,env->currentBall.pos.x,env->currentBall.pos.y);
				if(GoaliePosition(robot3,env->currentBall.pos.x+18,GBOTY-3))
					AngleOfPosition(robot3,env->currentBall.pos.x,env->currentBall.pos.y);
			}
			else
			{
				if(GoaliePosition(robot1,env->currentBall.pos.x+8,GBOTY+2))
					AngleOfPosition(robot2,env->currentBall.pos.x,env->currentBall.pos.y);
				if(GoaliePosition(robot3,env->currentBall.pos.x+18,GTOPY+3))
					AngleOfPosition(robot3,env->currentBall.pos.x,env->currentBall.pos.y);
			}
		}
		if(leng3<leng1 && leng3<leng2)
		{
			Shoot1_Right(robot3,env);
			if(env->currentBall.pos.y>(FTOP+FBOT)/2)
			{
				if(GoaliePosition(robot1,env->currentBall.pos.x+8,GTOPY-2))
					AngleOfPosition(robot1,env->currentBall.pos.x,env->currentBall.pos.y);
				if(GoaliePosition(robot2,env->currentBall.pos.x+18,GBOTY-3))
					AngleOfPosition(robot2,env->currentBall.pos.x,env->currentBall.pos.y);
			}
			else
			{
				if(GoaliePosition(robot1,env->currentBall.pos.x+8,GBOTY+2))
					AngleOfPosition(robot1,env->currentBall.pos.x,env->currentBall.pos.y);
				if(GoaliePosition(robot2,env->currentBall.pos.x+18,GTOPY+3))
					AngleOfPosition(robot2,env->currentBall.pos.x,env->currentBall.pos.y);
			}
		}		
	}

	else
	{
		if(leng1<=leng2 && leng1<=leng3)
		{
			Defence1_Right(robot1,env);
			GoaliePosition(robot2,robot1->pos.x-3,robot1->pos.y-10);
			GoaliePosition(robot3,robot1->pos.x,robot1->pos.y-20);
		}
		if(leng2<leng1 && leng2<=leng3)
		{
			Defence1_Right(robot2,env);
			GoaliePosition(robot1,robot2->pos.x,robot2->pos.y-10);
			GoaliePosition(robot3,robot2->pos.x,robot2->pos.y+10);
		}
		if(leng3<leng1 && leng3<leng2)
		{
			Defence1_Right(robot3,env);
			GoaliePosition(robot1,robot3->pos.x,robot3->pos.y+20);
			GoaliePosition(robot2,robot3->pos.x-3,robot3->pos.y+10);
		}
	}
}

void MoonAttack ( Robot *robot, Environment *env )
{
	PredictBall ( env );
	Position(robot, env->predictedBall.pos.x, env->predictedBall.pos.y);
}

void MoonFollowOpponent ( Robot *robot, OpponentRobot *opponent )
{
	Position(robot, opponent->pos.x, opponent->pos.y);
}

void Velocity ( Robot *robot, int vl, int vr )
{
	robot->velocityLeft = vl;
	robot->velocityRight = vr;
}

// robot soccer system p329
void Angle ( Robot *robot, int desired_angle)//原始Angle函数,有问题
{
	int theta_e, vl, vr;
	theta_e = desired_angle - (int)robot->rotation;
	
	while (theta_e > 180) theta_e -= 360;
	while (theta_e < -180) theta_e += 360;

	if (theta_e < -90) theta_e += 180;
	else if (theta_e > 90) theta_e -= 180;

	if (abs(theta_e) > 50) 
	{
		vl = (int)(-9./90.0 * (double) theta_e);
		vr = (int)(9./90.0 * (double)theta_e);
	}
	else if (abs(theta_e) > 20)
	{
		vl = (int)(-11.0/90.0 * (double)theta_e);
		vr = (int)(11.0/90.0 * (double)theta_e);//20度以下没有处理
	}
	Velocity (robot, vl, vr);
}

void Angle1(Robot *robot,int desired_angle)//让机器人原地旋转到某一角度,不分前后
{
	int theta_e, vl, vr;
	double Kp=1.2;
	
	theta_e = desired_angle - (int)robot->rotation;
	
	while (theta_e > 180) theta_e -= 360;
	while (theta_e < -180) theta_e += 360;

	if (theta_e < -90) theta_e += 180;	
	else if (theta_e > 90) theta_e -= 180;

	vl = (int)(0 - Kp*theta_e);
	vr = (int)(0 + Kp*theta_e);

	Velocity (robot, vl, vr);	
}

void AngleOfPosition(Robot *robot,double x, double y)//让机器人原地旋转始终对着某一点,不分前后
{
	int  vl, vr;
	double dx,dy,d_e,desired_angle,theta_e;
	double Kp=0.8;

	dx=x-robot->pos.x;
	dy=y-robot->pos.y;
	d_e=sqrt(dx*dx+dy*dy);

	if(dx==0 && dy==0)	
	{
		vl=0;
		vr=0;
		Velocity(robot,vl,vr);
	}
	
	else
		desired_angle = (int)(180./PI*atan2((double)(dy),(double)(dx)));
	
	theta_e = desired_angle-(int)robot->rotation;
	
	while(theta_e >180)  theta_e -= 360;
	while(theta_e <-180) theta_e += 360;

	if (theta_e < -90) theta_e += 180;	
	else if (theta_e > 90) theta_e -= 180;

	vl = (int)(0 - Kp*theta_e);
	vr = (int)(0 + Kp*theta_e);
 
	Velocity(robot, vl, vr);
}

void Position( Robot *robot, double x, double y )//原始Position函数,两方向前进,有问题,不能精确定位,但能保证直线速度
{
	int desired_angle = 0, theta_e = 0, d_angle = 0, vl, vr, vc = 70;

	double dx, dy, d_e, Ka = 10.0/90.0;
	dx = x - robot->pos.x;
	dy = y - robot->pos.y;

	d_e = sqrt(dx * dx + dy * dy);
	if (dx == 0 && dy == 0)//
		desired_angle = 90;
	else
		desired_angle = (int)(180. / PI * atan2((double)(dy), (double)(dx)));
	theta_e = desired_angle - (int)robot->rotation;
	
	while (theta_e > 180) theta_e -= 360;
	while (theta_e < -180) theta_e += 360;

	if (d_e > 100.) 
		Ka = 17. / 90.;
	else if (d_e > 50)
		Ka = 19. / 90.;
	else if (d_e > 30)
		Ka = 21. / 90.;
	else if (d_e > 20)
		Ka = 23. / 90.;
	else 
		Ka = 25. / 90.;
	
	if (theta_e > 95 || theta_e < -95)
	{
		theta_e += 180;
		
		if (theta_e > 180) 
			theta_e -= 360;
		if (theta_e > 80)
			theta_e = 80;
		if (theta_e < -80)
			theta_e = -80;
		if (d_e < 5.0 && abs(theta_e) < 40)
			Ka = 0.1;
		vr = (int)(-vc * (1.0 / (1.0 + exp(-3.0 * d_e)) - 0.3) + Ka * theta_e);//距离为零时,速度不为零