decisionmakingx.cpp

足球机器人仿真组SimuroSot11vs11的源程序。

			if(m_posBall.GetY() < PITCH_WIDTH/2 - PENALTY_AREA_WIDTH/2)
			{
				posTarget.SetY(PITCH_WIDTH/2 - GOAL_AREA_WIDTH/2 + glError);
				goToken = 1; 

				SendMsg(1, "1 posTarget %2.2f", posTarget.GetY());
			}
			else if(m_posBall.GetY() > PITCH_WIDTH/2 + PENALTY_AREA_WIDTH/2)
			{
				posTarget.SetY(PITCH_WIDTH/2 + GOAL_AREA_WIDTH/2 - glError);
				goToken = 1;

				SendMsg(1, "1 posTarget %2.2f", posTarget.GetY());
			}
			// 小禁区里，门区外
			else if(m_posBall.GetY() < PITCH_WIDTH/2 - GOAL_WIDTH/2 
					&& m_posBall.GetY() > PITCH_WIDTH/2 + GOAL_WIDTH/2)
			{
				posTarget.SetY(posBall.GetY());
				goToken = 1;

				SendMsg(1, "1 posTarget %2.2f", posTarget.GetY());
			}
			// 大禁区和小禁区之间
			else
			{
				posTarget.SetY(posBall.GetY());
				
				goToken = 0;
				
				SendMsg(1, "0 posTarget %2.2f", posTarget.GetY());
			}
		}
	}
	// 离门<30厘米
	else if(m_posBall.GetX() < GOAL_AREA_LENGTH + 10)
	{
		SendMsg(1, "X < 30");

		if(bIsGate)
		{   
#ifdef SimuroSot5
			posTarget.SetY(yBall);
#else
			posTarget.SetY(AdjustTarget());
#endif

			goToken = 0;

			SendMsg(1, "0 posTarget %2.2f", posTarget.GetY());
		}
		else
		{
			// 小禁区外
			if(m_posBall.GetY() < PITCH_WIDTH/2 - GOAL_AREA_WIDTH/2)
			{
				posTarget.SetY( PITCH_WIDTH/2 - GOAL_WIDTH/2  + gtError);
				goToken = 1;

				SendMsg(1, "1 posTarget %2.2f", posTarget.GetY());
			}
			else if(m_posBall.GetY() > PITCH_WIDTH/2 + GOAL_AREA_WIDTH/2)
			{
				posTarget.SetY( PITCH_WIDTH/2 + GOAL_WIDTH/2  - gtError);
				goToken = 1;

				SendMsg(1, "1 posTarget %2.2f", posTarget.GetY());
			}
			else
			{
				posTarget.SetY( posBall.GetY());
				goToken = 0;

				SendMsg(1, "0 posTarget %2.2f", posTarget.GetY());
			}
		}
	}
	// 离门<45厘米
	else if(m_posBall.GetX() < PENALTY_AREA_LENGTH + 10)
	{
		SendMsg(1, "X < 45");

		if(bIsGate) 	
		{
#ifdef SimuroSot5
			posTarget.SetY(yBall);
#else
			posTarget.SetY(AdjustTarget());
#endif
			goToken = 0;

			SendMsg(1, "0 posTarget %2.2f", posTarget.GetY());
		}
		else
		{
			// 不在门区
			if(ball.y < PITCH_WIDTH/2 - GOAL_WIDTH/2)
			{
				posTarget.SetY(PITCH_WIDTH/2 - GOAL_WIDTH/2 + 2*gtError);
				goToken = 2;

				SendMsg(1, "2 posTarget %2.2f", posTarget.GetY());
			}
			else if(ball.y > PITCH_WIDTH/2 + GOAL_WIDTH/2) 
			{
				posTarget.SetY(PITCH_WIDTH/2 + GOAL_WIDTH/2 - 2*gtError);
				goToken = 2;

				SendMsg(1, "2 posTarget %2.2f", posTarget.GetY());
			}
			else
			{
				posTarget.SetY(posBall.GetY());
				goToken = 0;

				SendMsg(1, "0 posTarget %2.2f", posTarget.GetY());
			}
		}
	}
	// 离门<65厘米
	else if(m_posBall.GetX() < 65)
	{				
		SendMsg(1, "X < 65");
		if(bIsGate)
		{
#ifdef SimuroSot5
			posTarget.SetY(yBall);
#else
			posTarget.SetY(AdjustTarget());
#endif
			
			goToken = 0;

			SendMsg(1, "0 posTarget %2.2f", posTarget.GetY());
		}
		else
		{
			if(m_posBall.GetY() < PITCH_WIDTH/2 - GOAL_WIDTH/2)
			{
				posTarget.SetY(PITCH_WIDTH/2 - GOAL_WIDTH/2 + 3*gtError);
				goToken = 2;

				SendMsg(1, "2 posTarget %2.2f", posTarget.GetY());
			}
			else if(m_posBall.GetY() > PITCH_WIDTH/2 + GOAL_WIDTH/2) 
			{
				posTarget.SetY(PITCH_WIDTH/2 + GOAL_WIDTH/2 - 3*gtError);
				goToken = 2;

				SendMsg(1, "2 posTarget %2.2f", posTarget.GetY());
			}
			else 
			{
				posTarget.SetY(posBall.GetY());
				goToken = 2;

				SendMsg(1, "2 posTarget %2.2f", posTarget.GetY());
			}
		}
	}
	//  离门>65厘米
	else 
	{
		SendMsg(1, "X > 65");

		if(bIsGate)
		{
#ifdef SimuroSot5
			posTarget.SetY(yBall);
#else
			posTarget.SetY(AdjustTarget());
#endif

			goToken = 2;

			SendMsg(1, "2 posTarget %2.2f", posTarget.GetY());
		}
		else
		{
			if(ball.y < PITCH_WIDTH/2 - GOAL_WIDTH/2)
			{
				posTarget.SetY(PITCH_WIDTH/2 - GOAL_WIDTH/2 + 4*gtError);
				goToken = 2;
				
				SendMsg(1, "2 posTarget %2.2f", posTarget.GetY());
			}
			else if(ball.y > PITCH_WIDTH/2 + GOAL_WIDTH/2) 
			{
				posTarget.SetY(PITCH_WIDTH/2 + GOAL_WIDTH/2 - 4*gtError);
				goToken = 2;
				
				SendMsg(1, "2 posTarget %2.2f", posTarget.GetY());
			}
			else 
			{
				posTarget.SetY(PITCH_WIDTH/2);
				goToken = 2;
				
				SendMsg(1, "2 posTarget %2.2f", posTarget.GetY());
			}
		}
	}
/*	// 如果在球门内，调整位置，封角度
	if (bIsGate)
	{
		if (ballCharacter.angle > PI/2)
		{
			posTarget.SetY(posTarget.GetY() - ROBOT_LENGTH);
		}
		else
		{
			posTarget.SetY(posTarget.GetY() + ROBOT_LENGTH);
		}
	}
*/
	if (m_posBall.GetX()<=15)			
	{
		posTarget.SetY(m_posBall.GetY());
	}
	VecPosition posRobot(Robot[nRobot].x, Robot[nRobot].y);
	double dist = (posRobot - posTarget).GetMagnitude();
	double distBall = (posRobot - m_posBall).GetMagnitude();

	//speed = ballCharacter.velocity * 60;
	//speed = ballCharacter.velocity * 60;
	speed = ballCharacter.velocity * 70;
	switch(goToken)
	{
	case	0:
		if(dist < 2)
			ToPositionPDGoal(nRobot, posTarget,speed,0);
		else
			ToPositionPDGoal(nRobot, posTarget,speed,50);
		break;
	case	1:
		if(dist < 2)
			ToPositionPDGoal(nRobot, posTarget,speed,0);
		else
			ToPositionPDGoal(nRobot, posTarget,speed,30);
		break;
	case	2:
		ToPositionPDGoal(nRobot, posTarget,speed, 30);
		break;
/*	case	3:
		if(dist < 2)
			ToPositionPDGoal(pRobot, posTarget,speed,0.0,pSpeed);
		else
		{
			if(distBall > 60)
			{
				if(ballCharacter.velocity < 8) 
					speed = gVMAX - 20.0;
				else 
					speed = gVMAX - 15.0;

				if(dist > 20)
					speed = gVMAX - 10.0;
			}
			else
			{	
				if(ballCharacter.velocity > 12.5 
					|| dist > 13) 
					speed = gVMAX;
			}

			if(ballCharacter.velocity > 15 
				&& bIsGate) 
				speed = gVMAX ;
			if(m_posBall.GetX() < 10 
				&& bIsGate)
				speed = gVMAX ;

			ToPositionPDGoal(pRobot, posTarget,speed,40,pSpeed);
		}
		break;
	case	4:
		MoveToPt(pRobot,  m_posBall, speed, pSpeed);
		break;
*/	default:
		::MessageBox(NULL, "GoalAction No goToken", "Error", MB_OK);
		break;
	}
	
	dbLRWheelVelocity* pSpeed = &rbV[nRobot];

/*	if( dist < 5)
	{
		if (fabs(fabs(Robot[nRobot].theta) - PI/2) > PI/180.*1.)
		{
			if(Robot[nRobot].theta < 0)
				TurnToAnglePD(&Robot[nRobot],-PI/2,NOCLOCK,pSpeed);
			else
				TurnToAnglePD(&Robot[nRobot],PI/2,NOCLOCK,pSpeed);
		}
	}
*/	

	VecPosition posBallTarget;
	if (m_posBall.GetY() > PITCH_WIDTH/2)
		posBallTarget.SetVecPosition(2*ROBOT_LENGTH+G_OFFSET, PITCH_WIDTH-30);
	else
		posBallTarget.SetVecPosition(2*ROBOT_LENGTH+G_OFFSET, 30);

	if (m_posBall.GetX() < 2*ROBOT_LENGTH + G_OFFSET)
	{
		// 大禁区外
		if (m_posBall.GetY() > PITCH_WIDTH/2 + PENALTY_AREA_WIDTH/2)
		{
			pSpeed->LeftValue = pSpeed->RightValue = 0;
			SendMsg(1, "Turn0");

			posBallTarget.SetVecPosition(ROBOT_LENGTH+G_OFFSET, PITCH_WIDTH/2 + PENALTY_AREA_WIDTH/2 - 5);
		//	ToPositionPD(pRobot, posBallTarget, MAXSPEED, pSpeed);
		}
		else if (m_posBall.GetY() < PITCH_WIDTH/2 - PENALTY_AREA_WIDTH/2)
		{
			pSpeed->LeftValue = pSpeed->RightValue = 0;
			SendMsg(1, "Turn0");

			posBallTarget.SetVecPosition(ROBOT_LENGTH+G_OFFSET, PITCH_WIDTH/2 - PENALTY_AREA_WIDTH/2 + 5);
		//	ToPositionPD(pRobot, posBallTarget, MAXSPEED, pSpeed);
		}
		
		// 球在小禁区外面
		else if (m_posBall.GetY() > PITCH_WIDTH/2 + GOAL_WIDTH/2)
		{
			SendMsg(1, "Turn1");

			posBallTarget.SetVecPosition((posRobot.GetX()+m_posBall.GetX())/2, m_posBall.GetY());
			ToPositionPD(nRobot, posBallTarget, MAXSPEED);
		}
		else if (m_posBall.GetY() < PITCH_WIDTH/2 - GOAL_WIDTH/2)
		{
			SendMsg(1, "Turn2");
			
			posBallTarget.SetVecPosition((posRobot.GetX() +m_posBall.GetX())/2, m_posBall.GetY());
			ToPositionPD(nRobot, posBallTarget, MAXSPEED);
		}
		// 球门内
		else 
		{
			// 球几乎在球门内
			if (m_posBall.GetX() < BALL_SIZE)
			{
			
				//key = dmDEG.DEPenaltyDirection;
				//ToPositionNew(nRobot, posBallTarget, m_posBall, 50, 2);
				ToPositionNew(nRobot, posBallTarget, m_posBall, 70, 2);
				SendMsg(1, "ToPositionNew1");
			}
			
			// 守门员在球前面, 护球
			if (m_posBall.GetX() < BALL_SIZE + 1
				&& m_posBall.GetX() + BALL_SIZE < posRobot.GetX() - ROBOT_LENGTH + 2)
			{
				if (m_posBall.GetDistanceTo(posRobot) < ROBOT_LENGTH+BALL_SIZE)
				{
					VecPosition posBallTarget(posRobot.GetX(), m_posBall.GetY());
					ToPositionPDGoal(nRobot, posBallTarget, 80, 20);
					SendMsg(1, "Huqiu");
				}
				else
				{
					//ToPositionNew(nRobot, posBallTarget, m_posBall, 50, 2);
					ToPositionNew(nRobot, posBallTarget, m_posBall, 70, 2);
					SendMsg(1, "ToPositionNew2");
				}
				
			}
			
			// 球在守门员前面，旋转破坏球
			if (m_posBall.GetX() - BALL_SIZE > posRobot.GetX() + ROBOT_LENGTH - 2
				&& m_posBall.GetX() - BALL_SIZE < posRobot.GetX() + ROBOT_LENGTH
				&& m_posBall.GetY() < posRobot.GetY() + ROBOT_LENGTH + 1
				&& m_posBall.GetY() > posRobot.GetY() - ROBOT_LENGTH - 1)
			{
				if (m_posBall.GetY() > PITCH_WIDTH/2)
					Turn(nRobot, MAXSPEED, ANTICLOCK);
				else
					Turn(nRobot, MAXSPEED, CLOCKWISE);
				
				SendMsg(1, "Turn");
			}
		}
	}
	
	SendMsg(1, "Final posTarget %2.2f %2.2f %2.2f %2.2f %2.2f", 
		posTarget.GetX(), posTarget.GetY(), speed, pSpeed->LeftValue, pSpeed->RightValue);
/*	////////////////     撞墙处理     /////////////////////////////
	// 出门区
	if(Robot[nRobot].y > PITCH_WIDTH/2 + GOAL_WIDTH/2 
		|| Robot[nRobot].y < PITCH_WIDTH/2 - GOAL_WIDTH/2 )
	{
		double theta = VecPosition::NormalizeAngle2PI(Robot[nRobot].theta);
		if(theta < PI/6 || (theta > PI && theta - PI < PI/6))
			TurnToAnglePD(pRobot,PI/2,ANTICLOCK,pSpeed);
		else if((2*PI - theta) < PI/6 || (theta < PI && PI - theta < PI/6))
			TurnToAnglePD(pRobot,PI/2,CLOCKWISE,pSpeed);
	}

	////////////////////  正面球处理   ////////////////////////////////
	if(distBall < 8)
	{
		if(m_posBall.GetX() > Robot[nRobot].x 
			&& fabs(ball.y - Robot[nRobot].y) < 8 )
		{
			if(Robot[nRobot].theta > PI)
			{
				if(m_posBall.GetY() > Robot[nRobot].y)
					TurnToAnglePD(pRobot,PI/2,CLOCKWISE,pSpeed);
				else
					TurnToAnglePD(pRobot,PI/2,ANTICLOCK,pSpeed);
			}
			else
			{
				if(m_posBall.GetY() > Robot[nRobot].y)
					TurnToAnglePD(pRobot,3*PI/2,CLOCKWISE,pSpeed);
				else
					TurnToAnglePD(pRobot,3*PI/2,ANTICLOCK,pSpeed);
			}
		}
	}
*/
}

int CDecisionMakingx::TurnToAnglePDli(dbROBOTPOSTURE *pRobot,double dbAngle,int clock,dbLRWheelVelocity *pSpeed)
//本函数用于使小车的方向以给定的时钟方向快速转到所要求的角度方向
//Robot为小车的位置信息，Angle为需要转向的角度，Speed为返回的左右轮速
//pPD为比例、微分调节参数结构
{
	double Difference,SameSpeed;
	int Quadrant;
	Difference=pRobot->theta-dbAngle;
    Difference = cn_AngleTrim2PI(Difference);
	if (Difference < m_AngleParameter.AngleError)//判断是否在角度误差限之内
	{
		pSpeed->LeftValue=0.;
		pSpeed->RightValue=0.;
		return 1;
	}
	if (clock==ANTICLOCK)
		Difference=2*PI-Difference;
	else if (clock == NOCLOCK)
	{
		if (Difference >= 0 &&  Difference < PI/2)//判断角度差所在象限
			Quadrant=1;
		else if (Difference >= PI/2 &&  Difference < PI)
		{
			Quadrant=2;
			Difference=PI-Difference;
		}
		else if (Difference >= PI && Difference < 3*PI/2) 
		{
			Quadrant=3;
			Difference=Difference-PI;
		}
		else
		{
			Quadrant=4;
			Difference=2*PI-Difference;
		}
	}
	//此处进行PD调节
    SameSpeed=m_AngleParameter.Kp*Difference+m_AngleParameter.Kd*(Difference-m_Front);
	if (SameSpeed>m_AngleParameter.MaxAngleSpeed)
		SameSpeed=m_AngleParameter.MaxAngleSpeed;
	m_Front=Difference;
	if (clock==CLOCKWISE)
	{
		pSpeed->LeftValue=SameSpeed