kick.cpp

机器人足球的基本动作函数

void Kick2(Environment *env , int  robot , Vector3D ToPos )
{
	Mydata * p;
	p=(Mydata *)env->userData;
	
	double LimitedCircle = 3;
	Vector3D ball = Meetball_p(env,robot);	//use the predictball position

	Vector3D RobotToBall;		//人和球的相对位置
	RobotToBall.x = ball.x - p->robot[robot].pos.x ;
	RobotToBall.y = ball.y - p->robot[robot].pos.y ;
	RobotToBall.z = Atan(p->robot[robot].pos , ball);
	
	Vector3D BallToGate ;		//球和球门的相对位置
	BallToGate.x = ToPos.x - ball.x ;
	BallToGate.y = ToPos.y - ball.y ;
	BallToGate.z = Atan(ball , ToPos);
	
	double gateangle=BallToGate.z;

	double RunAngle ;
	RunAngle = RobotToBall.z - BallToGate.z;
	RegulateAngle(RunAngle);
	
	double dis= Distance(ball,p->robot[robot].pos);

	if(dis > 3*LimitedCircle){
		Vector3D Center;
		if(RunAngle >0){
			BallToGate.z -=90;
		}
		else{
			BallToGate.z +=90;
		}
		
		RegulateAngle(BallToGate.z);
		Center.x = ball.x + LimitedCircle*cos(BallToGate.z /180.0);
		Center.y = ball.y + LimitedCircle*sin(BallToGate.z /180.0);
		Center.z = 0;
		double distance = Distance(Center,p->robot[robot].pos);
		
		if(distance < 2*LimitedCircle)
		{
			RunAngle = RobotToBall.z + RunAngle /2;	// 可以调整  2 
		}
		else{
			double CenAngle= Atan(p->robot[robot].pos,Center);
			if(RunAngle <0){
				RunAngle = CenAngle-180*LimitedCircle*asin(LimitedCircle/distance)/3.142;
				RegulateAngle(RunAngle);
			}
			else{
				RunAngle = CenAngle+180*LimitedCircle*asin(LimitedCircle/distance)/3.142;
				RegulateAngle(RunAngle);
			}
		}
		
	}
	else{

		
		RunAngle = RobotToBall.z + RunAngle /2;	// 可以调整  2 
		RegulateAngle(RunAngle);
	}
	double paraA=gateangle - p->robot[robot].rotation;
	if(paraA<0){
		paraA=-paraA;
	}
	if(paraA>90){
		paraA=180-paraA;
	}
	if(0.1>paraA)
	{
		paraA=0.1;
	}
	double paraB=125*dis/3*LimitedCircle*10/paraA;
	if(paraB>125){
		paraB=125;
	}
	PAngle(env,robot,RunAngle,paraB);
	
}