strategy.cpp

足球机器人比赛程序

		vl = (int)(-vc * (1.0 / (1.0 + exp(-3.0 * d_e)) - 0.3) - Ka * theta_e);
	}
	
	else if (theta_e < 85 && theta_e > -85)
	{
		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);
		vl = (int)( vc * (1.0 / (1.0 + exp(-3.0 * d_e)) - 0.3) - Ka * theta_e);
	}

	else
	{
		vr = (int)(+.17 * theta_e);
		vl = (int)(-.17 * theta_e);
	}
	Velocity(robot, vl, vr);
}
 
void Position0( Robot *robot, double x, double y )//改造原始Position函数,改为单向前进,提速为125,有边角调整
{
	if( robot->pos.x<=6.8118+5 || robot->pos.x>=93.4259-5 || robot->pos.y<=6.3730+5 || robot->pos.y>=77.2392-5 )
	{//保证边角不被锁死
		Position1( robot, x, y );
		return;
	}

	int desired_angle = 0, theta_e = 0, d_angle = 0, vl, vr, vc = 120;

	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 = 1.0;  //17. / 90.;
	else if (d_e > 50)
		Ka = 0.8;  //19. / 90.;
	else if (d_e > 30)
		Ka = 0.7;  //21. / 90.;
	else if (d_e > 20)
		Ka = 0.5;  //23. / 90.;
	else 
		Ka = 0.3;  //25. / 90.;

	if (d_e < 5.0 && abs(theta_e) < 20)
		Ka = 0.1;//0.1;

	vr = (int)( vc * (1.0 / (1.0 + exp(-3.0 * d_e)) - 0.15) + Ka * theta_e);
	vl = (int)( vc * (1.0 / (1.0 + exp(-3.0 * d_e)) - 0.15) - Ka * theta_e);
	
	if(abs(vl)>=abs(vr))
	{
		if(vl>0)
		{
			vl=125;
			vr=125.0*vr/vl;
		}
		if(vl<0)
		{
			vl=-125;
			vr=-125.0*vr/vl;
		}
	}
	if(abs(vl)<abs(vr))
	{
		if(vr>0)
		{
			vr=125;
			vl=125.0*vl/vr;
		}
		if(vr<0)
		{
			vr=-125;
			vl=-125.0*vl/vr;
		}
	}
	Velocity(robot, vl, vr);
}

void Position1( Robot *robot, double x, double y )//改造原始Position函数,提高直线和转弯速度,提速至125,两方向前进
{
	int vl, vr, vc = 120 ;
	double desired_angle = 0, theta_e = 0, d_angle = 0 ;

	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 = 1.2;  //17. / 90.;
	else if (d_e > 50)
		Ka = 0.9;  //19. / 90.;
	else if (d_e > 30)
		Ka = 0.8;  //21. / 90.;
	else if (d_e > 20)
		Ka = 0.7;  //23. / 90.;
	else 
		Ka = 0.6;  //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.5;//0.1;
		vr = (int)(-vc * (1.0 / (1.0 + exp(-3.0 * d_e)) - 0.2) + Ka * theta_e);//距离为零时,速度不为零
		vl = (int)(-vc * (1.0 / (1.0 + exp(-3.0 * d_e)) - 0.2) - Ka * theta_e);
	}
	
	else if (theta_e < 85 && theta_e > -85)
	{
		if (d_e < 5.0 && abs(theta_e) < 40)
			Ka = 0.5;//0.1;
		vr = (int)( vc * (1.0 / (1.0 + exp(-3.0 * d_e)) - 0.2) + Ka * theta_e);
		vl = (int)( vc * (1.0 / (1.0 + exp(-3.0 * d_e)) - 0.2) - Ka * theta_e);
	}

	else
	{
		vr = (int)(+.8 * theta_e);
		vl = (int)(-.8 * theta_e);
	}

	if(abs(vl)>=abs(vr))
	{
		if(vl>0)
		{
			vl=125;
			vr=125.0*vr/vl;
		}
		if(vl<0)
		{
			vl=-125;
			vr=-125.0*vr/vl;
		}
	}
	if(abs(vl)<abs(vr))
	{
		if(vr>0)
		{
			vr=125;
			vl=125.0*vl/vr;
		}
		if(vr<0)
		{
			vr=-125;
			vl=-125.0*vl/vr;
		}
	}
	Velocity(robot, vl, vr);
}
//改造原始Position函数,提高直线和转弯速度,提速至125,两方向前进
//进一步提高直线和适当减小转弯速度--cz
void Position1_cz( Robot *robot, double x, double y )
{
	int vl, vr, vc = 120 ;
	double desired_angle = 0, theta_e = 0, d_angle = 0 ;
	double f_d, d_e, dx, dy, Ka = 5.0/90.0, kd = -250;//kd = -200;

	f_d=1.0 / (1.0 + exp(-0.3 * d_e))- 0.5;
	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 = 1.2;  //17. / 90.;
	else if (d_e > 50)
		Ka = 0.9;  //19. / 90.;
	else if (d_e > 30)
		Ka = 0.8;  //21. / 90.;
	else if (d_e > 20)
		Ka = 0.7;  //23. / 90.;
	else 
		Ka = 0.6;  //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.5;//0.1;
		vr = (int)(-kd * f_d + Ka * theta_e);//距离为零时,速度不为零
		vl = (int)(-kd * f_d - Ka * theta_e);
	}
	
	else if (theta_e < 85 && theta_e > -85)
	{
		if (d_e < 5.0 && abs(theta_e) < 40)
			Ka = 0.5;//0.1;
		vr = (int)( kd * f_d + Ka * theta_e);
		vl = (int)( kd * f_d - Ka * theta_e);
	}

	else
	{
		vr = (int)(+.8 * theta_e);
		vl = (int)(-.8 * theta_e);
	}

	Velocity(robot, vl, vr);
}

bool Position2(Robot *robot,double x, double y)//将GoaliePosition提速至125,两方向前进
{
	int desired_angle=0, theta_e = 0, vl, vr;
	double dx, dy, d_e, Kd, Ka;//, dx1, dy1, dx2, dy2;

	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;

	Kd = 8;  //0.85;		//This is the Robot's overall speed.
	Ka = 10./90.;

	if( d_e<0.1 )
	{
		vl=0;
		vr=0;
		Velocity(robot, vl, vr);
		return TRUE;
	}
	
	if( d_e>=0.1 && theta_e > -90 && theta_e < 90) 
//	if( theta_e > -90 && theta_e < 90)
	{
		d_e=sqrt(dx*dx+dy*dy);
		vr=vl=(int)(d_e*Kd);
		if ( vl > 120 ) vl = 120;
		if ( vr > 120 ) vr = 120;
		if ( vl < 1 ) vl = 1;
		if ( vr < 1 ) vr = 1;

		if ( d_e < 2 || ( theta_e >= -20 && theta_e <=20 ) ) 
		{
			if ( d_e < 1 || ( theta_e >= -35 && theta_e <=35 ) )
			{
				Ka=0.22;//0.12;
			}
			else
			{
//				vl = 20;
//				vr = 20;
				Ka=0.32;//0.22;	
			}								
		}
		else
		{
//			vl = 35;
//			vr = 35;
			Ka=0.35;//0.25;
		}
		vl = (int)(vl - (Ka*theta_e));
		vr = (int)(vr + (Ka*theta_e));
	}

	if( d_e>=0.1 && (theta_e <= -90 || theta_e >= 90) )
//	if( theta_e <= -90 || theta_e >= 90 )
	{
		d_e=sqrt(dx*dx+dy*dy);
		vr=vl=(int)(d_e*-Kd);
		if ( vr < -120 ) vr = -120;
		if ( vl < -120 ) vl = -120;
		if ( vr > -1 ) vr = -1;
		if ( vl > -1 ) vl = -1;
		
//		if(theta_e<-90)		theta_e += 180;
//		else if(theta_e>90)	theta_e -= 180;

		if ( d_e < 2 || ( theta_e >= -20 && theta_e <=20 ) ) 
		{
			if ( d_e <1 || ( theta_e >= -35 && theta_e <=35 ) ) 
			{
				Ka = 0.22;//0.12;
			}
			else 
			{
//				vl = -20;
//				vr = -20;
				Ka = 0.32;//0.22;
			}
		}
		else 
		{
//			vl = -35;
//			vr = -35;
			Ka = 0.35;//0.25;
		}
		vl = (int)(vl - (Ka*theta_e));
		vr = (int)(vr + (Ka*theta_e));
	}

	if(abs(vl)>=abs(vr))
	{
		if(vl>0)
		{
			vl=125;
			vr=125.0*vr/vl;
		}
		if(vl<0)
		{
			vl=-125;
			vr=-125.0*vr/vl;
		}
	}
	if(abs(vl)<abs(vr))
	{
		if(vr>0)
		{
			vr=125;
			vl=125.0*vl/vr;
		}
		if(vr<0)
		{
			vr=-125;
			vl=-125.0*vl/vr;
		}
	}
	Velocity(robot, vl, vr);
	return FALSE;
}

bool GoaliePosition(Robot *robot,double x, double y)
{												
	int desired_angle=0, theta_e = 0, vl, vr;
	double dx, dy, d_e, Kd, Ka;//, dx1, dy1, dx2, dy2;

	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;

	Kd = 8;  //0.85;		//This is the Robot's overall speed.
	Ka = 10./90.;

	if( d_e<0.1 )
	{
		vl=0;
		vr=0;
		Velocity(robot, vl, vr);
		return TRUE;
	}
	
	if( d_e>=0.1 && theta_e > -90 && theta_e < 90) 
//	if( theta_e > -90 && theta_e < 90)
	{
		d_e=sqrt(dx*dx+dy*dy);
		vr=vl=(int)(d_e*Kd);
		if ( vl > 120 ) vl = 120;
		if ( vr > 120 ) vr = 120;
		if ( vl < 1 ) vl = 1;
		if ( vr < 1 ) vr = 1;

		if ( d_e < 2 || ( theta_e >= -20 && theta_e <=20 ) ) 
		{
			if ( d_e < 1 || ( theta_e >= -35 && theta_e <=35 ) )
			{
				Ka=0.22;//0.12;
			}
			else
			{
//				vl = 20;
//				vr = 20;
				Ka=0.32;//0.22;	
			}								
		}
		else
		{
//			vl = 35;
//			vr = 35;
			Ka=0.35;//0.25;
		}
		vl = (int)(vl - (Ka*theta_e));
		vr = (int)(vr + (Ka*theta_e));
	}

	if( d_e>=0.1 && (theta_e <= -90 || theta_e >= 90) )
//	if( theta_e <= -90 || theta_e >= 90 )
	{
		d_e=sqrt(dx*dx+dy*dy);
		vr=vl=(int)(d_e*-Kd);
		if ( vr < -120 ) vr = -120;
		if ( vl < -120 ) vl = -120;
		if ( vr > -1 ) vr = -1;
		if ( vl > -1 ) vl = -1;
		
		if(theta_e<-90)		theta_e += 180;
		else if(theta_e>90)	theta_e -= 180;

		if ( d_e < 2 || ( theta_e >= -20 && theta_e <=20 ) ) 
		{
			if ( d_e <1 || ( theta_e >= -35 && theta_e <=35 ) ) 
			{
				Ka = 0.22;//0.12;
			}
			else 
			{
//				vl = -20;
//				vr = -20;
				Ka = 0.32;//0.22;
			}
		}
		else 
		{
//			vl = -35;
//			vr = -35;
			Ka = 0.35;//0.25;
		}
		vl = (int)(vl - (Ka*theta_e));
		vr = (int)(vr + (Ka*theta_e));
	}
	Velocity(robot, vl, vr);
	return FALSE;
}

void PredictBall ( Environment *env )//预测球的位置(一步)
{
	double dx = env->currentBall.pos.x - env->lastBall.pos.x;
	double dy = env->currentBall.pos.y - env->lastBall.pos.y;
	env->predictedBall.pos.x = env->currentBall.pos.x + dx;
	env->predictedBall.pos.y = env->currentBall.pos.y + dy;
}

void Goalie1 ( Robot *robot, Environment *env )//原始守门员,只能以270度方向做直线运动
{
	double velocityLeft = 0, velocityRight = 0;
	
	double Tx = env->goalBounds.right - env->currentBall.pos.x;
	double Ty = env->fieldBounds.top - env->currentBall.pos.y;

	double Ax = env->goalBounds.right - robot->pos.x;
	double Ay = env->fieldBounds.top - robot->pos.y;

	if ( Ay > Ty + 0.9 && Ay > 27 )
	{
		velocityLeft = -100;
		velocityRight = -100;
	}

	if ( Ay < Ty - 0.9 && Ay < 43 )
	{
		velocityLeft = 100;
		velocityRight = 100;