miro99.cpp

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

#define		EST_TX		9
#define		EST_TY		6

void My_Strategy()
{	
	static int test=0;
	count++;
	preY.add(count, PositionOfBall[1]);
	preX.add(count, PositionOfBall[0]);

	//Angle(HOME2, 90);
	//Position(HOME1, PositionOfBall[0], PositionOfBall[1]);
	//Shoot(HOME1, PositionOfBall[0], PositionOfBall[1], PositionOfBall[0]+10., PositionOfBall[1]);
	//Goalie(HOME1);
	//Kick(HOME1);
	//AKick(HOME1);
	//Attack(HOME1, HOME2);
	
	//Shoot(HOME1, PositionOfBall[0], PositionOfBall[1], PositionOfBall[0]+10, PositionOfBall[1]);
	//AvoidBound(HOME1);
	//Game();	
	Auto_Position();

	//GPosition(HOME1, 20,100);
}


#define KICK_OFF		0
#define PENALTY_KICK	1
#define FREE_KICK		2
#define FREE_BALL_1		3
#define FREE_BALL_2		4
#define FREE_BALL_3		5
#define FREE_BALL_4		6
#define GOALIE_KICK2	7
#define GAME			8

void Auto_Position()
{
 	switch(MODE1){
	case KICK_OFF:
		Auto_Robot_Position(HOME1,0,40,100);
		Auto_Robot_Position(HOME2,120,75,60);
		Auto_Robot_Position(HGOALIE,90,5,65);
		break;
 	case PENALTY_KICK:
		Auto_Robot_Position(HOME1,0,95,65);
		Auto_Robot_Position(HOME2,0,70,100);
		Auto_Robot_Position(HGOALIE,90,5,65);
		break;
    case FREE_KICK:
		Auto_Robot_Position(HOME1,45,20,40);
		Auto_Robot_Position(HOME2,-45,20,90);
		Auto_Robot_Position(HGOALIE,90,5,65);
		break;
	case FREE_BALL_1:  
		Auto_Robot_Position(HOME1,0,90,105);
		Auto_Robot_Position(HOME2,0,75,60);
		Auto_Robot_Position(HGOALIE,90,5,65);
		break;
	case FREE_BALL_2:  
		Auto_Robot_Position(HOME1,0,20,105);
		Auto_Robot_Position(HOME2,0,20,60);
		Auto_Robot_Position(HGOALIE,90,5,65);
 		break;
	case FREE_BALL_3:
		Auto_Robot_Position(HOME1,0,20,25);
		Auto_Robot_Position(HOME2,0,20,70);
		Auto_Robot_Position(HGOALIE,90,5,65);
		break;
	case FREE_BALL_4:  
		Auto_Robot_Position(HOME1,0,90,25);
		Auto_Robot_Position(HOME2,0,75,70);
		Auto_Robot_Position(HGOALIE,90,5,65);
		break;
	case GOALIE_KICK2:
		Auto_Robot_Position(HOME1,0,8,65);
		Auto_Robot_Position(HOME2,0,65,100);
		Auto_Robot_Position(HGOALIE,90,5,20);
		break;

	case GAME:
		Game();
	//	m_nTest = FALSE;
		
		break;
	
 
	} 


}


#define KICK_OFF		0
#define PENALTY_KICK	1
#define FREE_KICK		2
#define FREE_BALL		3
#define GOALIE_KICK		4
#define DEFENSE			5
void Game()
{
	double dx1, dy1, dx2, dy2;
	
	// First Kicker is HOME2
	switch(MODE){
	case KICK_OFF:
		dx1 = PositionOfBall[0]-PositionOfHomeRobot[HOME2][0];
		dy1 = PositionOfBall[1]-PositionOfHomeRobot[HOME2][1];
		if(dx1*dx1+dy1*dy1 <6.0*6.0){
			Velocity(HOME2, 34, 15);
			Position(HOME1, PositionOfBall[0], PositionOfBall[1]);
			Goalie(HGOALIE);
		}
		else{
			MODE = DEFENSE;
			InitRole();
			Goalie(HGOALIE);
		}
		break;

	
	case PENALTY_KICK:
		dx1 = PositionOfBall[0]-PositionOfHomeRobot[HOME1][0];
		dy1 = PositionOfBall[1]-PositionOfHomeRobot[HOME1][1];
			
		if(dx1*dx1+dy1*dy1<8.0*8.0){
			Shoot(HOME1, PositionOfBall[0], PositionOfBall[1], 150, 83);
			Goalie(HGOALIE);
		}
		else if(dx1*dx1+dy1*dy1> 8.7*8.7 && dx1*dx1+dy1*dy1 < 10.0*10.0){
			Shoot(HOME1, PositionOfBall[0], PositionOfBall[1], 150, 83);
			Goalie(HGOALIE);
		}
		else{
			MODE = DEFENSE;
			InitRole();
			Goalie(HGOALIE);
		}
		break;

	
	case FREE_KICK:
		dx1 = PositionOfBall[0]-PositionOfHomeRobot[HOME1][0];
		dy1 = PositionOfBall[1]-PositionOfHomeRobot[HOME1][1];
		if(dx1*dx1+dy1*dy1>10.0*10.0){
			Position(HOME1, PositionOfBall[0], PositionOfBall[1]);
			Goalie(HGOALIE);
		}
		else{
			MODE = DEFENSE;
			InitRole();
			Goalie(HGOALIE);
		}
		break;


	case FREE_BALL:  // Consier Free Ball Mode. It's a curious!!!!
		dx1 = PositionOfBall[0]-PositionOfHomeRobot[HOME1][0];
		dy1 = PositionOfBall[1]-PositionOfHomeRobot[HOME1][1];
		dx2 = PositionOfBall[0]-PositionOfHomeRobot[HOME2][0];
		dy2 = PositionOfBall[1]-PositionOfHomeRobot[HOME2][1];

		if(PositionOfBall[1]>55.0 && PositionOfBall[1]<80.0 && dx1*dx1+dy1*dy1>10.0*10.0){
			Shoot(HOME1, PositionOfBall[0], PositionOfBall[1], PositionOfBall[0]+5.0, PositionOfBall[1]);
			Goalie(HGOALIE);
		}
		else if(PositionOfBall[1]<35.0 && PositionOfBall[1] > 10.0 && dx2*dx2+dy2*dy2>10.0*10.0){
			Shoot(HOME2, PositionOfBall[0], PositionOfBall[1], PositionOfBall[0]+5.0, PositionOfBall[1]);
			Goalie(HGOALIE);
		}
		else{
			MODE = DEFENSE;
			InitRole();
			Goalie(HGOALIE);
		}
		break;

	case GOALIE_KICK:
		dx1 = PositionOfBall[0]-PositionOfHomeRobot[HOME1][0];
		dy1 = PositionOfBall[1]-PositionOfHomeRobot[HOME1][1];
		
		if(dx1*dx1+dy1*dy1 < 11.0 * 11.0){
			Shoot(HOME1, PositionOfBall[0], PositionOfBall[1], 150.0, 80.0);
			Goalie(HGOALIE);
		}
		
	
		else{
			MODE = DEFENSE;
			InitRole();
			Goalie(HGOALIE);
		}

		break;

	case DEFENSE:
		MODE = DEFENSE;
		Attack(HOME1, HOME2);
		Goalie(HGOALIE);
		break;
	}	
}


void InitRole()
{
	Attack(HOME1, HOME2);
}


void Attack(int whichrobot1, int whichrobot2)
{
	if(PositionOfBall[0]>151.0 || PositionOfBall[0]<-1.0)
		StopRobot();
		
	if(PositionOfBall[1] > 55.0){
		Kick(whichrobot1);
		Position(whichrobot2, PositionOfBall[0]-20.0, 40.0);//17.0
	}
	else if(PositionOfBall[1]<75.0){
		Kick(whichrobot2);			
		Position(whichrobot1, PositionOfBall[0]-20.0, 90.0);//17.0				
	}	
	
}


void Send_Command()
{
	m_pComm.WriteCommBlock((LPSTR)command, 8);
}



void StopRobot()
{
	 
	command[0] = 0x00;
	command[1] = 0x7f;
	command[2] = 0x7f;
	command[3] = 0x7f;
	command[4] = 0x7f;
	command[5] = 0x7f;
	command[6] = 0x7f;
	command[7] = 0x00;

	m_pComm.WriteCommBlock((LPSTR)command, 8);
}


void WhichRobotStop(int whichrobot)
{

	command[0] = 0x00;
	switch(whichrobot){
	case HOME1 : 
		command[1] = 0x7f;
		command[2] = 0x7f;
		break;
	case HOME2:
		command[3] = 0x7f;
		command[4] = 0x7f;
		break;
	case HGOALIE:
		command[5] = 0x7f;
		command[6] = 0x7f;
		break;
	}
	command[7] = 0x00;
}



void Velocity(int whichrobot, int vl, int vr)
{


	vr = -vr;
	vl = -vl;

	if ( vr > 125 ) vr = 125;
	if ( vl > 125 ) vl = 125;
	if ( vr < -125) vr = -125;
	if ( vl < -125) vl = -125;

	vl += 127;				// 加档甫 0 - 255 荤捞狼 蔼栏肺 官槽促.
	vr += 127;				
 
	if(vr == 0xAA)
		vl = 0xAB;				// 抗距绢牢 0xAA甫 乔窍扁 困秦辑
	
	if(vl == 0xAA)
		vl = 0xAB;
	
	command[0] = 0x00;
	
	switch(whichrobot){
	case HOME1 : 

		command[1] = (unsigned char)vr;
		command[2] = (unsigned char)vl;
		break;
	case HOME2:
		command[3] = (unsigned char)vr;
		command[4] = (unsigned char)vl;
		break;
	case HGOALIE:
		command[5] = (unsigned char)vr;
		command[6] = (unsigned char)vl;
		break;
	}
	
	command[7] = 0x00;
	
}



void GPosition(int whichrobot, double x, double y)
{
	int desired_angle,theta_e,d_angle=0,vl,vr;
	double dx,dy,distance_e,Kd=1.0, Ka=0.15, Kad=1.3, Ki=0.2;//Ki = 0.1;
	double K1= 2.0, K2=1.5;
		
	dx = x-PositionOfHomeRobot[whichrobot][0];
	dy = y-PositionOfHomeRobot[whichrobot][1];

	distance_e = sqrt(1.0*dx*dx+1.0*dy*dy);
															   //nice			//Nice far distance					
	if(distance_e > 100)		Ka = 0.27, Kad = 0.3;//(0.5, 0.12, 0.3), (0.5,0.12, 0.17) 0.5,Kd = 0.7, 0.14, 0.1(0.2)
	else if(distance_e > 50)	Ka = 0.27, Kad = 0.2;//(0.8, 0.11, 0.3), (0.6, 0.12, 0.16),0.5, 0.9, 0.14, 0.1  // 0.9 0.11 0.13
	else if(distance_e > 30)	Ka = 0.25, Kad = 0.3;//(1.0, 0.1,  0.3), (0.7, 0.12, 0.16),0.6, 1.3, 0.14, 0.1  // 1.3 0.11 0.13
	else if(distance_e > 20)	Ka = 0.2,  Kad = 0.3;//(1.0,  0.1,  0.3), (0.74, 0.11, 0.16),0.8, 1.8, 0.16, 0.1  // 1.8 0.12 0.12
	else if(distance_e > 10)	Ka = 0.2,  Kad = 0.3;//(1.0, 0.1,  0.3), (0.76,0.11,0.16), 1.1, 2.4, 0.16, 0.1 // 2.4 0.12 0.12
	else						Ka = 0.2, Kad = 0.3;//(1.0,  0.1,  0.3), (0.8,0.11, 0.16), 1.3, 3.2, 0.16  0.1 //3.2 0.11 0.11

	if(distance_e < 10.)
		distance_e = K1*2*distance_e;
	else
		distance_e = K1*(distance_e-10.) + 20.* K1;

	Ka = Ka*K2;

	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 > 95 || theta_e < -95) {
		theta_e += 180;
		if (theta_e>180) theta_e -= 360;
		d_angle = theta_e - tmp_angle[whichrobot];
		if(d_angle>60)	d_angle = 60;
		else if(d_angle<-60)	d_angle = -60;
		else d_angle = d_angle;
		
		vr = (int)(-distance_e + 0.7*Ka*theta_e + 2.7*Kad*d_angle + .6*Ki*angle_sum);
		vl = (int)(-distance_e - 0.7*Ka*theta_e - 2.7*Kad*d_angle + .6*Ki*angle_sum);
		
	}
	
	else if(theta_e < 85 && theta_e > -85){
		d_angle = theta_e - tmp_angle[whichrobot];
		if(d_angle>60)	d_angle = 60;
		else if(d_angle<-60)	d_angle = -60;
		else d_angle = d_angle;
		
		vr = (int)(distance_e + 0.7*Ka*theta_e + 2.7*Kad*d_angle + .6*Ki*angle_sum);
		vl = (int)(distance_e - 0.7*Ka*theta_e - 2.7*Kad*d_angle + .6*Ki*angle_sum);
	}
	else{
		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)(-9 +.12*K2*theta_e);	// .13,0.05
			vl = (int)(-9 -.12*K2*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)(9 +.12*K2*theta_e);	// .13,0.05
			vl = (int)(9 -.12*K2*theta_e);	// -.13			
			}
		else
			{
			vr = (int)(+.13*K2*theta_e);	// +.13, 0.05
			vl = (int)(-.13*K2*theta_e);	// -.13	
			}
	}

	tmp_angle[whichrobot] = theta_e;//(line tracking is test for desired_angle	

	angle_sum += theta_e;
	angle_sum = (int)(angle_sum/20);
	Velocity(whichrobot, vl, vr);

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


void Shoot(int whichrobot, double px, double py, double tx, double ty)
{
	
	double dx1, dy1, dx2, dy2, Kp = 45.0, Ka = 0.22, d = 5.5, Kad = 0.3;//40.0, 0.35//40,0.36
	int a1, a2, a3, vl, vr, theta_e, theta_d, d_angle=0;		
	double K1=1.60;

	dx1 = tx - px;
	dy1 = ty - py;

	if( (PositionOfHomeRobot[whichrobot][1] > py + 0.5 * d )&& (PositionOfHomeRobot[whichrobot][0] > px) )
		dy1 += 0.5*d;
	else if( (PositionOfHomeRobot[whichrobot][1] < py - 0.5*d) && (PositionOfHomeRobot[whichrobot][0] > px) )
		dy1 -= 0.5*d;
	if(dx1 == 0 && dy1 == 0)	a1 = 90;
	else						a1 = (int)(180/M_PI*atan2((double)(dy1), (double)(dx1)));
	
	dx2 = px - PositionOfHomeRobot[whichrobot][0];
	dy2 = py - PositionOfHomeRobot[whichrobot][1];
	//d_x = dx2, d_y = dy2;
	
	if(dx2==0 && dy2==0)	a2 = 90;
	else				a2 = (int)(180/M_PI*atan2((double)(dy2),(double)(dx2)));

	a3 = 180 - a1 + 2*a2;
	
	if((PositionOfHomeRobot[whichrobot][1] > py - 0.5*d) && (PositionOfHomeRobot[whichrobot][1] < py +0.5*d) && (PositionOfHomeRobot[whichrobot][0] > px) )
		a3 = 180;

	theta_e = a3-AngleOfHomeRobot[whichrobot];
	theta_d = a2-AngleOfHomeRobot[whichrobot];

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

	if(theta_e < -90){
		theta_e += 180;
		Kp = -Kp;
		}
	else if(theta_e > 90){
		theta_e -= 180;
		Kp = -Kp;
		}
	d_angle = theta_e - tmp_angle[whichrobot];
	
	tmp_angle[whichrobot] = theta_e;