strategysystem.cpp

来自「足球机器人仿真组SimuroSot11vs11的源程序。」· C++ 代码 · 共 857 行 · 第 1/2 页

#include "StdAfx.h"
#include "StrategySystem.h"
//#include "Communications.h"

IMPLEMENT_DYNAMIC(CStrategySystem, CObject)

extern int nKick;


#define  BALL_WIDTH		78//30
#define  BALL_LENGTH	156//60 
#define  BALL_DIS	    26//10 
#define  CORNER         115//45

CStrategySystem::CStrategySystem(int id)
{
	m_OurTeam=id;

		boundRect.SetRect(65,95,965,723);//场地四角

	if(id)
		m_nGameArea=GAME_LEFT;
	else
		m_nGameArea=GAME_RIGHT;
	for(int i=0;i<35;i++)
		command[i]=0;
/*	C_Home1.Data.Lv=0;
	C_Home1.Data.Rv=0;
	C_Home1.Data.Command=C_STOP;
	C_Home2.Data.Lv=0;
	C_Home2.Data.Rv=0;
	C_Home2.Data.Command=C_STOP;
	C_Home3.Data.Lv=0;
	C_Home3.Data.Rv=0;
	C_Home3.Data.Command=C_STOP;
	C_Home4.Data.Lv=0;
	C_Home4.Data.Rv=0;
	C_Home4.Data.Command=C_STOP;
	C_Home5.Data.Lv=0;
	C_Home5.Data.Rv=0;
	C_Home5.Data.Command=C_STOP;
	C_Home6.Data.Lv=0;
	C_Home6.Data.Rv=0;
	C_Home6.Data.Command=C_STOP;
	C_Home7.Data.Lv=0;
	C_Home7.Data.Rv=0;
	C_Home7.Data.Command=C_STOP;
	C_Home8.Data.Lv=0;
	C_Home8.Data.Rv=0;
	C_Home8.Data.Command=C_STOP;
	C_Home9.Data.Lv=0;
	C_Home9.Data.Rv=0;
	C_Home9.Data.Command=C_STOP;
	C_Home10.Data.Lv=0;
	C_Home10.Data.Rv=0;
	C_Home10.Data.Command=C_STOP;
	C_Home11.Data.Lv=0;
	C_Home11.Data.Rv=0;
	C_Home11.Data.Command=C_STOP;*/

	m_nStrategy=id;//left=1,right=0
	nKick=0;
	nKick2=0;
	nShoot=0;
	nSweep=0;
}



CStrategySystem::~CStrategySystem()
{

}

void CStrategySystem::Action()
{
	Think();
}

void CStrategySystem::Think()
{
	NormalGame5();
}


void CStrategySystem::NormalGame5()
{
	int i;
	i=C_CheckBallPos();
	Position(HOME1,ball.position);
}


void CStrategySystem::Angle(int which, int desired_angle)
{
	OurRobot *robot;
	int theta_e, vL, vR; 
	
	switch(which){
	case HOME1:	
		robot = &home1;
		break;
	case HOME2:            
		robot = &home2;
		break;
	case HOME3:	
		robot = &home3;
		break;
	case HOME4:	
		robot = &home4;
		break;
	case HOME5:	
		robot = &home5;
		break;
	case HOME6:	
		robot = &home6;
		break;
	case HOME7:	
		robot = &home7;
		break;
	case HOME8:	
		robot = &home8;
		break;
	case HOME9:	
		robot = &home9;
		break;
	case HOME10:	
		robot = &home10;
		break;
	case HGOALIE:      
		robot = &hgoalie;
		break;
	}

	//计算目标角与机器人方向角的差
	theta_e = desired_angle - robot->angle;

	//角度规范化在（-360，360）
	while(theta_e > 180)
		theta_e -= 360;
	while(theta_e < -180)
		theta_e += 360;                                      

	//若旋转的角度大于90或小于-90，旋转其补角，并改变正方向
	if(theta_e < -90)  
		theta_e += 180; 
	else if(theta_e > 90)  
		theta_e -= 180;		

	//比例系数是如何确定的？
	vL = (int)(60.0/90.0*theta_e);  
	vR = (int)(-60.0/90.0*theta_e);
	Velocity(which, vL, vR);
}

void CStrategySystem::Velocity(int which, int vL, int vR)
{
	//检查变量范围
	if(vL < -127)	vL = -127;
	if(vL > 127)	vL = 127;

	if(vR < -127)	vR = -127;
	if(vR > 127)	vR = 127;
	
	
	switch(which){
	case HOME1:
		command[2] = vL;
		command[3] = vR;
		command[4] = C_GO;
		break;
	case HOME2:           
		command[5] = vL;
		command[6] = vR;
		command[7] = C_GO;	
		break;
	case HOME3:           
		command[8] = vL;
		command[9] = vR;
		command[10] = C_GO;	
		break;
	case HOME4:           
		command[11] = vL;
		command[12] = vR;
		command[13] = C_GO;	
		break;
	case HOME5:           
		command[14] = vL;
		command[15] = vR;
		command[16] = C_GO;	
		break;
	case HOME6:           
		command[17] = vL;
		command[18] = vR;
		command[19] = C_GO;	
		break;
	case HOME7:           
		command[20] = vL;
		command[21] = vR;
		command[22] = C_GO;	
		break;
	case HOME8:           
		command[23] = vL;
		command[24] = vR;
		command[25] = C_GO;	
		break;
	case HOME9:           
		command[26] = vL;
		command[27] = vR;
		command[28] = C_GO;	
		break;
	case HOME10:           
		command[29] = vL;
		command[30] = vR;
		command[31] = C_GO;	
		break;
	case HGOALIE:         
		command[32] = vL;
		command[33] = vR;
		command[34] = C_GO;	
		break;
	}
	
}


void CStrategySystem::Position(int which, CPoint point)
{
	OurRobot *robot;    
	double distance_e;                 
	int dx, dy, desired_angle, theta_e, vL, vR;
	
	switch(which){
	case HOME1:	
		robot = &home1;
		break;
	case HOME2:            
		robot = &home2;
		break;
	case HOME3:	
		robot = &home3;
		break;
	case HOME4:	
		robot = &home4;
		break;
	case HOME5:	
		robot = &home5;
		break;
	case HOME6:	
		robot = &home6;
		break;
	case HOME7:	
		robot = &home7;
		break;
	case HOME8:	
		robot = &home8;
		break;
	case HOME9:	
		robot = &home9;
		break;
	case HOME10:	
		robot = &home10;
		break;
	case HGOALIE:      
		robot = &hgoalie;
		break;
	}
        
	dx = point.x - robot->position.x;
	dy = point.y - robot->position.y;
	                                            
	distance_e = sqrt(1.0*dx*dx+1.0*dy*dy);	                                            

    if(dx == 0 && dy == 0)
		desired_angle = 90;
	else
		desired_angle = (int)(180.0/M_PI*atan2((double)(dy), (double)(dx)));
	
	theta_e = desired_angle - robot->angle;

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

	//若夹角在（90，180）或（-180，-90）之间，旋转其补角
	//倒退计算出的距离
	if(theta_e < -90){  
		theta_e += 180; 
		distance_e = -distance_e;		
	}
	else if(theta_e > 90){  
		theta_e -= 180;		
		distance_e = -distance_e;
	}

	
	vL = (int)(2.*(100.0/1000.0*distance_e+40.0/90.0*theta_e));
	vR = (int)(2.*(100.0/1000.0*distance_e-40.0/90.0*theta_e));
	

//	TRACE("\nin strategy vL=%d,vR=%d",vL,vR);
	Velocity(which, vL, vR);  
}


#define G_OFFSET		20//15
#define G_ANGLE_BOUND	60
#define G_BOUND_BOUND	10

void CStrategySystem::Goalie(int which)
{
	CPoint point;      
	OurRobot *robot;
	int estimate_x, estimate_y;//, angle;
	int height,center;

	switch(which){
	case HOME1:
		robot = &home1;
		break;
	case HOME2:    
		robot = &home2;
		break;
	case HGOALIE:
		robot = &hgoalie;
		break;
	}

	height = boundRect.bottom-boundRect.top;
	center = (boundRect.bottom+boundRect.top)/2;

    /*if(boundRect.right-ball.position.x  >= 60*3){
		//the Ball is far from the gate
		estimate_x = boundRect.right-G_OFFSET;
   		estimate_y = ball.position.y;
   	}
	else if(boundRect.right-ball.position.x  >= 15*3){
		//the Ball is in middle distance
		estimate_x = boundRect.right-G_OFFSET;
    	estimate_y = ball.position.y;
	}
    else{
		//近距离处理直接奔向球（应为球后面，否则极容易弹进门）
		estimate_x = boundRect.right-G_OFFSET;
   		estimate_y = ball.position.y;
    }*/
	if(boundRect.right-ball.position.x  >= 20*3)
	{
		estimate_x = boundRect.right-G_OFFSET;
    	estimate_y = ball.position.y;
	}
	else
	{
		estimate_x = boundRect.right - G_OFFSET;
		CPoint position;
		position=C_NearestOppFromBall();
		double K,b;
		K=(position.y-ball.position.y)/(position.x-ball.position.x);
		b=position.y-K*position.x;
		estimate_y=(int)(K*estimate_x+b);
	}
	//如果超出了球门区，则限定在球门去的两边
	if(estimate_y < boundRect.top+height*1/3)
		estimate_y = boundRect.top+height*1/3;
	else if(estimate_y > boundRect.bottom-height*1/3)
		estimate_y = boundRect.bottom-height*1/3;

	GoaliePosition(which, CPoint(estimate_x, estimate_y));
}

void CStrategySystem::ReceiveData(Ball bal,OurRobot ho1,OurRobot ho2,OurRobot ho3,OurRobot ho4,
		             OurRobot ho5,OurRobot ho6,OurRobot ho7,OurRobot ho8,OurRobot ho9,
					 OurRobot ho10,OurRobot hgo,Opponent opp)
{
	//我方在右半区，直接接收
	if(m_nGameArea==GAME_RIGHT)
	{
		ball.position=bal.position;         //球	
		
		home1.position=ho1.position;        //HOME1
		home1.angle=ho1.angle;
		
		home2.position=ho2.position;        //HOME2
		home2.angle=ho2.angle;
		
		home3.position=ho3.position;
		home3.angle=ho3.angle;

		home4.position=ho4.position;
		home4.angle=ho4.angle;

		home5.position=ho5.position;
		home5.angle=ho5.angle;

		home6.position=ho6.position;
		home6.angle=ho6.angle;

		home7.position=ho7.position;
		home7.angle=ho7.angle;

		home8.position=ho8.position;
		home8.angle=ho8.angle;

		home9.position=ho9.position;
		home9.angle=ho9.angle;

		home10.position=ho10.position;
		home10.angle=ho10.angle;

		hgoalie.position=hgo.position;      //HGOALIE
		hgoalie.angle=hgo.angle;
		
		opponent.position1=opp.position1;   //OPPONENT
		opponent.position2=opp.position2;
		opponent.position3=opp.position3;
		opponent.position4=opp.position4;
		opponent.position5=opp.position5;
		opponent.position6=opp.position6;
		opponent.position7=opp.position7;
		opponent.position8=opp.position8;
		opponent.position9=opp.position9;
		opponent.position10=opp.position10;
		opponent.position11=opp.position11;
	
	}
	else  //若我方在左半区，传给策略时作相应变换
	{
		//球、球员及对手信息位置关于场地中心对称