miro99.cpp

FIRA 3对3 机器人足球PC端控制程序。

	if(abs(theta_e) > 50){	
		Ka = 0.16*K1;//0.22;//0.26
	}
	else if(abs(theta_e > 40)) {
		Ka = 0.18*K1;//0.2;//0.22;//0.2;//0.22;
	}
	else if(abs(theta_e > 30)) {
		Ka = 0.2*K1;//0.22;//0.27;//0.18;//0.27;
	}
	else if(abs(theta_e > 20)) {
		Ka = 0.22*K1;//0.24;//0.3;//0.16;//0.3;
	}
	else	{
		Ka = 0.24*K1;//0.28;//0.32;//0.14;//0.32
	}

	if(dx2*dx2+dy2*dy2<10.*10. && abs(theta_d)<30)
		Ka = 0.13*K1;

	vl = (int)(Kp + Ka*(double)theta_e);// + Kad*d_angle);
	vr = (int)(Kp - Ka*(double)theta_e);// - Kad*d_angle);


	if( (theta_e<95 && theta_e>85) || (theta_e>-95 && theta_e<-85) )
		{
		if( (((130.0-PositionOfHomeRobot[whichrobot][1])<4.0)&&(AngleOfHomeRobot[whichrobot]>40)&&(AngleOfHomeRobot[whichrobot]<140)) || ((PositionOfHomeRobot[whichrobot][1]<4.0)&&(AngleOfHomeRobot[whichrobot]>220)&&(AngleOfHomeRobot[whichrobot]<320)) )
			{
			vr = (int)(-12+.16*K1*theta_e);	// .13,0.05
			vl = (int)(-12-.16*K1*theta_e);	// -.13			
			}
		else if( (((130.0-PositionOfHomeRobot[whichrobot][1])<4.0)&&(AngleOfHomeRobot[whichrobot]>220)&&(AngleOfHomeRobot[whichrobot]<320)) || ((PositionOfHomeRobot[whichrobot][1]<4.0)&&(AngleOfHomeRobot[whichrobot]>40)&&(AngleOfHomeRobot[whichrobot]<140)) )
			{
			vr = (int)(12 +.16*K1*theta_e);	// .13,0.05
			vl = (int)(12 -.16*K1*theta_e);	// -.13			
			}
		else
			{
			vr = (int)(+.19*K1*theta_e);	// 0.05
			vl = (int)(-.19*K1*theta_e);
			}
		}
	else
		{
		vl = -vl;
		vr = -vr;
		}
	
	Velocity(whichrobot,vl,vr);
	
	AvoidBound(whichrobot);

	if(FlagHomeRobotFound[whichrobot] == FALSE)
		WhichRobotStop(whichrobot);

}

#define ANGLE_BOUND 60//30
#define BOUND_BOUND 7.5
void AvoidBound(int whichrobot)
{
	int angle_r,angle,theta_e, vl, vr;
	double dx, dy; 
	double K1=2.0, K2=1.5;

	dx=PositionOfBall[0]-PositionOfHomeRobot[whichrobot][0];
	dy=PositionOfBall[1]-PositionOfHomeRobot[whichrobot][1];

	if(dx==0 && dy==0)	theta_e = 90;
	else				theta_e = (int)(180/M_PI*atan2((double)(dy),(double)(dx)));

	angle_r = AngleOfHomeRobot[whichrobot];
	theta_e -= AngleOfHomeRobot[whichrobot];
	while(angle_r>180) angle_r -= 360;
	while(angle_r<-180) angle_r += 360;

	while(theta_e >180)  theta_e -= 360;
	while(theta_e <-180) theta_e += 360;

	//Top bottom side
	if(angle_r> -90-ANGLE_BOUND && angle_r < -90+ANGLE_BOUND && PositionOfHomeRobot[whichrobot][1] > 130.0 - BOUND_BOUND ){
		vr = (int)(14.*K1 +19.0/90.0*K2*theta_e);//16		// Top, r_angle = -90
		vl = (int)(14.*K1 -19.0/90.0*K2*theta_e);		
		Velocity(whichrobot, vl, vr);
	}
	else if(angle_r > 90-ANGLE_BOUND && angle_r < 90+ANGLE_BOUND && PositionOfHomeRobot[whichrobot][1] > 130.0 - BOUND_BOUND ){
		vr = (int)(-14.*K1 +19.0/90.0*K2*theta_e);			// Top, r_angle = 90
		vl = (int)(-14.*K1 -19.0/90.0*K2*theta_e);		
		Velocity(whichrobot, vl, vr);
	}
		
	else if(angle_r > -90-ANGLE_BOUND && angle_r < -90+ANGLE_BOUND && PositionOfHomeRobot[whichrobot][1] < 0.0 + BOUND_BOUND ){
		vr = (int)(-14.*K1 +19.0/90.0*K2*theta_e);	
		vl = (int)(-14.*K1 -19.0/90.0*K2*theta_e);		
		Velocity(whichrobot, vl, vr);
	}
	else if(angle_r > 90-ANGLE_BOUND && angle_r < 90+ANGLE_BOUND && PositionOfHomeRobot[whichrobot][1] < 0.0 + BOUND_BOUND ){
		vr = (int)(14.*K1 +19.0/90.0*K2*theta_e);	
		vl = (int)(14.*K1 -19.0/90.0*K2*theta_e);		
		Velocity(whichrobot, vl, vr);
	}
		

	//Left side
	angle = abs(angle_r);

	if(angle < ANGLE_BOUND && PositionOfHomeRobot[whichrobot][0] < 0.0+BOUND_BOUND && PositionOfHomeRobot[whichrobot][1] > 85.0){
		vr = (int)(14.*K1 +19.0/90.0*K2*theta_e);	
		vl = (int)(14.*K1 -19.0/90.0*K2*theta_e);		
		Velocity(whichrobot, vl, vr);
	}
	else if(angle > 180-ANGLE_BOUND && PositionOfHomeRobot[whichrobot][0] < 0.0+BOUND_BOUND && PositionOfHomeRobot[whichrobot][1] > 85.0){
		vr = (int)(-14.*K1 +19.0/90.0*K2*theta_e);	
		vl = (int)(-14.*K1 -19.0/90.0*K2*theta_e);		
		Velocity(whichrobot, vl, vr);
	}
		
	else if(angle < ANGLE_BOUND && PositionOfHomeRobot[whichrobot][0]< 0.0+BOUND_BOUND && PositionOfHomeRobot[whichrobot][1] < 45.0){
		vr = (int)(14.*K1 +19.0/90.0*K2*theta_e);	
		vl = (int)(14.*K1 -19.0/90.0*K2*theta_e);		
		Velocity(whichrobot, vl, vr);
	}	
	else if(angle > 180-ANGLE_BOUND && PositionOfHomeRobot[whichrobot][0]<0.0+BOUND_BOUND && PositionOfHomeRobot[whichrobot][1] < 45.0){
		vr = (int)(-14.*K1 +19.0/90.0*K2*theta_e);	
		vl = (int)(-14.*K1 -19.0/90.0*K2*theta_e);		
		Velocity(whichrobot, vl, vr);
	}
		
	//Right side
	if(angle < ANGLE_BOUND && PositionOfHomeRobot[whichrobot][0]>150.0-BOUND_BOUND && PositionOfHomeRobot[whichrobot][1] > 85.0){
		vr = (int)(-14.*K1 +19.0/90.0*K2*theta_e);	
		vl = (int)(-14.*K1 -19.0/90.0*K2*theta_e);		
		Velocity(whichrobot, vl, vr);
	}
		
	else if(angle > 180-ANGLE_BOUND && PositionOfHomeRobot[whichrobot][0]>150.0-BOUND_BOUND && PositionOfHomeRobot[whichrobot][1] > 85.0){
		vr = (int)(14.*K1 +19.0/90.0*K2*theta_e);	
		vl = (int)(14.*K1 -19.0/90.0*K2*theta_e);		
		Velocity(whichrobot, vl, vr);
	}
		
	else if(angle < ANGLE_BOUND && PositionOfHomeRobot[whichrobot][0]>150.0-BOUND_BOUND && PositionOfHomeRobot[whichrobot][1] < 45.0){
		vr = (int)(-14.*K1 + 19.0/90.0*K2*theta_e);	
		vl = (int)(-14.*K1 - 19.0/90.0*K2*theta_e);		
		Velocity(whichrobot, vl, vr);
	}
		
	else if(angle > 180-ANGLE_BOUND && PositionOfHomeRobot[whichrobot][0]>150.0-BOUND_BOUND && PositionOfHomeRobot[whichrobot][1] < 45.0){
		vr = (int)(14.*K1 +19.0/90.0*K2*theta_e);	
		vl = (int)(14.*K1 -19.0/90.0*K2*theta_e);		
		Velocity(whichrobot, vl, vr);
	}
		

	if(PositionOfBall[0] < 0.0)
		Velocity(whichrobot, 0,0);
}


void Kick(int whichrobot)
{	
	//************************* Before the 9 20 kick *****************
	switch(nKick){
		case 0:
			//Shoot(whichrobot, PositionOfBall[0], PositionOfBall[1], 150, 65);
			Shoot(whichrobot, PositionOfBall[0], PositionOfBall[1], PositionOfBall[0]+10.0, PositionOfBall[1]);
			if(PositionOfBall[0] < PositionOfHomeRobot[whichrobot][0])
				nKick = 1;
			break;
		case 1:
			if(PositionOfBall[1]>PositionOfHomeRobot[whichrobot][1])
				Position(whichrobot, PositionOfBall[0]-10.0, PositionOfBall[1]-10.0);
			else
				Position(whichrobot, PositionOfBall[0]-10.0, PositionOfBall[1]+10.0);

			if( PositionOfBall[0] > PositionOfHomeRobot[whichrobot][0] -10.0)
				nKick = 0;

			break;		
	}

}

#define		ESTIMATE_TX		12
#define		ESTIMATE_TY		6

#define G_OFFSET		6.0
#define G_ANGLE_BOUND	60
#define	G_BOUND_BOUND	4.0
#define G_LENGTH		45


#define G 			4//2.3//4.
#define D 			3//0.1//3.
#define A 			1.//1. 1.5 is good.
#define K_FIELD 	2.4   //2.5



void Auto_Robot_Position(int whichrobot, int desired_angle, double dx,double dy)
{
 		double dist_err;
		
		dist_err = sqrt((dx  - PositionOfHomeRobot[whichrobot][0])*(dx  - PositionOfHomeRobot[whichrobot][0])
				+(dy  - PositionOfHomeRobot[whichrobot][1])*(dy  - PositionOfHomeRobot[whichrobot][1]));


		if(dist_err > 4.0)
			Position (whichrobot, dx, dy);
		else
			Angle(whichrobot, desired_angle);
		if(FlagHomeRobotFound[whichrobot] == FALSE)
		WhichRobotStop(whichrobot);
}


void Position(int whichrobot, double x, double y)
{	
	int desired_angle=0, theta_e = 0, vl, vr, vc=50;
	double dx, dy, d_e, Ka=0.2,Kd = 1.5 ;  //1
    //double dx2,dy2 ;
	//int angle_bound;
	 
	dx=x-PositionOfHomeRobot[whichrobot][0];
	dy=y-PositionOfHomeRobot[whichrobot][1];
	d_e=sqrt(dx*dx+dy*dy);
	//d_x = dx, d_y = dy;
	//  芭府俊 蝶扼 急屈利栏肺 加档甫 函拳矫难霖促.
	
	//dist_err = dist_err/2.0;

	if (d_e > 20.0)
		d_e = 20.0;

	//if (d_e < 2.0 && d_e >-2. )
	//	d_e = 0.0;

    if (d_e < -20.0)
		d_e = -20.0;

	



	if(dx==0 && dy==0)	desired_angle = 90; 
	else				desired_angle = (int)(180./M_PI*atan2((double)(dy),(double)(dx)));
	
	theta_e = desired_angle-AngleOfHomeRobot[whichrobot];
	
	while(theta_e > 180)	theta_e -= 360;  //俊扼 阿档甫 累篮蔼栏肺 函券
	while(theta_e < -180)	theta_e += 360;
    if (theta_e >= 90){
		theta_e -= 180; 
		d_e = -d_e;
	}
	if (theta_e < -90){
		theta_e += 180; 
		d_e = -d_e;
	}

	vl = int(Kd*d_e - Ka*theta_e);
    vr = int(Kd*d_e + Ka*theta_e);

	Velocity(whichrobot, vl, vr);
	//AvoidBound(whichrobot);
    
	if(FlagHomeRobotFound[whichrobot] == FALSE)
		WhichRobotStop(whichrobot);
}


void Angle(int whichrobot, int desired_angle)//More thinking parameter and method 5.16
{
	int theta_e, vl, vr;

	theta_e = desired_angle - AngleOfHomeRobot[whichrobot];
	
 	while(theta_e >180)  theta_e -= 360;
 	while(theta_e <-180) theta_e += 360;

	vl = (int)(-25.0/90.0*(double)theta_e);
	vr = (int)(25.0/90.0*(double)theta_e);

	Velocity(whichrobot, vl, vr);
	if(FlagHomeRobotFound[whichrobot] == FALSE)
		WhichRobotStop(whichrobot);
}
////////////////////////////////////////////////////////////////////////////
////////////////////////////////////////////////////////////////////////////
////////////////////////////////////////////////////////////////////////////
//   GOALIE METHOD
////////////////////////////////////////////////////////////////////////////
////////////////////////////////////////////////////////////////////////////
////////////////////////////////////////////////////////////////////////////


#define Initial		4
#define CENTER_Y    65

void Goalie(int whichrobot)				
{

	double	est_x, est_y;
	double  delta_x=0, delta_y=0;

	if (PositionOfBall[0] > 15) 
	{
		est_x = Initial;
		est_y = PositionOfBall[1];

	} 
	else 
	{
		est_x = Initial;
		est_y = PositionOfBall[1];

	}

	if (est_y > 85.)
		est_y = 85.;
	if (est_y < 45.)
		est_y = 45.;

	if ( PositionOfBall[0] < Initial+3.0  && PositionOfBall[0] > 1.0 )
	{
		if (((PositionOfBall[1] > 85. && PositionOfBall[1] < 105.)||
			(PositionOfBall[1] < 45. && PositionOfBall[1] > 25.)) )
		{
			est_x = PositionOfBall[0]+1.0;
			est_y = PositionOfBall[1];
		}

		if (PositionOfBall[1] > 45.0 && PositionOfBall[1] < 85.0)
		{
			est_x = PositionOfBall[0]+1.0;
			est_y = PositionOfBall[1];
		}
	}

	FieldPosition4Goalie(whichrobot, est_x, est_y);
}

#define   Initial_x    4
void FieldPosition4Goalie(int whichrobot, double x, double y)
{

	double	dx, dy, dist_err;
	double	n_field, r_angle, e_theta;
	double	V_c;
	int		V_l, V_r;
	double  filtered_position[2];
	double  deacc=0.0;

    filtered_position[0] = PositionOfHomeRobot[whichrobot][0];
	filtered_position[1] = PositionOfHomeRobot[whichrobot][1];
	r_angle = AngleOfHomeRobot[whichrobot];

    // 抗螟痢苞 泅犁 肺嚎荤捞狼 芭府甫 螟沥茄促.
	dx = x-filtered_position[0];
	dy = y-filtered_position[1];
	dist_err = sqrt(dx*dx + dy*dy);

   if ((filtered_position[0] <Initial_x+1.0) && (filtered_position[0] > Initial_x-1.0) && dist_err < 3.0)
	{
		if ( dy < 0)
			n_field = 270.;
		else 
		    n_field = 90.;
	}
	else  if ( dx != 0) 
			n_field = 180./M_PI*atan2((double)(dy),(double)(dx));
	     else
		 {
			if ( dy < 0)
				n_field = 270.;
			else 
				n_field = 90.;
		 }
	      
	e_theta = n_field - r_angle;

    // e_theta狼 阿阑 180档 ~ -180档 官操绢 霖促. 
	while(e_theta > 180.)	
		e_theta -= 360.;
	while(e_theta <-180.)	
		e_theta += 360.;

	if ( dist_err > 5 && fabs(e_theta) < 25. &&
		(filtered_position[0] <Initial_x+1.5) && (filtered_position[0] > Initial_x-1.5) )
		e_theta = 0.0;
	
	if (dist_err > 12.0)
		dist_err = 12.0;

	if ( PositionOfBall[0] < Initial_x+4.0 )
	{
		if (((PositionOfBall[1] > 85. && PositionOfBall[1] < 105.)||
			(PositionOfBall[1] < 45. && PositionOfBall[1] > 25.)))
			dist_err = 3.0;
	}

	if ((PositionOfBall[0] < Initial_x+6.0) && dist_err > 4.
		&& PositionOfBall[1] > 45 && PositionOfBall[1] < 85.)
		dist_err = 15.0;

	if ( dist_err < 4. && prev_center_vc4goalie > 30 && (dist_err-prev_dist_err4goalie) < 0.0)
		deacc = 1.0;
	else
		deacc = 0.0;


	V_c = 4.5*dist_err + 10.*(dist_err - prev_dist_err4goalie)*deacc-5.0;
	
	
	if ( V_c < 1.0)
		V_c = 0.0;

	prev_dist_err4goalie = dist_err;

	//*****  剧规氢 函券阑 茄促 ****************
	if(e_theta < -100)
	{  
		e_theta += 180; 
		V_c = -V_c;
	}
	else if(e_theta >= 100)
	{  
		e_theta -= 180;		
		V_c = -V_c;
	}


//*****  剧 官柠狼 加档甫 阿 坷瞒, 邦伏阑 绊妨窍咯 拌魂茄促 ****************

	V_r = (int)(+35.0/90.0*e_theta + 20.0/90.0*(e_theta - prev_e_theta4goalie)*0.0 + V_c );   	
	V_l = (int)(-35.0/90.0*e_theta - 20.0/90.0*(e_theta - prev_e_theta4goalie)*0.0 + V_c );  
	prev_center_vc4goalie = (V_r + V_l)/2.0;

	r_angle = (double)(((int)r_angle+360)%360);

	if ( r_angle >150 && r_angle < 210 && PositionOfHomeRobot[whichrobot][0] < Initial_x  &&
		(PositionOfHomeRobot[whichrobot][1] < 45 || PositionOfHomeRobot[whichrobot][1] > 85)) 
	{
				V_r = (int)(-60.0/90.0*e_theta);
				V_l = (int)(60.0/90.0*e_theta);
	}	

	Velocity(whichrobot, (int)V_l, (int)V_r);

	prev_e_theta4goalie = e_theta;
}