decisionmakingx.cpp

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

// DecisionMakingx.cpp: implementation of the CDecisionMakingx class.
//
//////////////////////////////////////////////////////////////////////
#include "stdio.h"
#include "stdafx.h"
#include "DecisionMakingx.h"
#include "Geometry.h"
//#include "Realball.h"
//#include "BaseAgent.h"

#include "Matrix.h"
/*#ifndef realball
#include "Realball.h"
#endif
*/
//#define XIMAGE

#ifdef XIMAGE
#include "F:\Library\CxImage\CxImage\ximage.h"
#pragma comment(linker, "/libpath:\"F:\\Library\\CxImage\\lib\" ")
#pragma comment(lib, "F:\\Library\\CxImage\\lib\\cximageD.lib")
#pragma comment(lib, "F:\\Library\\CxImage\\lib\\j2kD.lib")
#pragma comment(lib, "F:\\Library\\CxImage\\lib\\jasperD.lib")
#pragma comment(lib, "F:\\Library\\CxImage\\lib\\jbigD.lib")
#pragma comment(lib, "F:\\Library\\CxImage\\lib\\jpegD.lib")
#pragma comment(lib, "F:\\Library\\CxImage\\lib\\pngD.lib")
#pragma comment(lib, "F:\\Library\\CxImage\\lib\\tiffD.lib")
#pragma comment(lib, "F:\\Library\\CxImage\\lib\\zlib.lib")
#endif

//////////////////////////////////////////////////////////////////////
// Construction/Destruction
//////////////////////////////////////////////////////////////////////

//define TEST1

void CDecisionMakingx::Test() 
{
//	HundunTest();
//	return;

	VecPosition pos(220, 90);
//	ToPositionN(&Robot[2], m_posBall, 110, &rbV[2]);

//	ToPositionPD(&Robot[2], m_posBall, 70, &rbV[2]);
//	TurnToAnglePD(&Robot[2], -2*PI/4, ANTICLOCK, &rbV[2]);
//	TurnToPointPD(&Robot[2], m_posBall, NOCLOCK, &rbV[2]);
//	MoveOnAngle(&Robot[2], -PI/4, 70, &rbV[2]);
//	MoveToPt(&Robot[2], m_posBall, 70, &rbV[2]);
//	ToPositionPDGoal(&Robot[4], m_posBall, 40, 0, &rbV[4]);
//	ToPositionNew(&Robot[2], pos, m_posBall, 110, 2, &rbV[2]);

//	if (m_nTimer < 10)
		rbV[2].LeftValue = rbV[2].RightValue = -110;
//	ShootLine(2);
//	Vect_MidShoot(2);
	
//	GoalieAction(4);
//	ToPositionPDGoal(&Robot[4], pos, 110, 0, &rbV[4]);
//	for (int i=0; i<11; i++)
	ToPositionN(3, m_posBall, 110);
//	ToPositionPDGoal(4, pos, 110, 0);
//	Wait(0, pos, 0, 110);
//	BoundPushBall(&Robot[3], &rbV[3]);

//	Robot[4].x = 5.54; Robot[4].y = 76.69; Robot[4].theta = -1.57;
//	pos.SetX(5.50); pos.SetY(72.00);
//	ToPositionPDGoal(&Robot[4], pos, MAXSPEED, 50, &rbV[4]);

//	Kick2(2);
}

void CDecisionMakingx::HundunTest()
{
	VecPosition	q[10];

	q[0] = m_posBall;
	q[1].SetVecPosition(Robot[2].x, Robot[2].y);
	q[2].SetVecPosition(Robot[0].x, Robot[0].y);
	q[3].SetVecPosition(Robot[3].x, Robot[3].y);
	q[4].SetVecPosition(Robot[4].x, Robot[4].y);
	
	ToPositionAvoidObstacles(1, q, 4);

//	LogData();//李

//	if (m_nTimer % 10 == 1)
//		Capture();	
}

CDecisionMakingx::CDecisionMakingx() :
	nettpt("Data\\tpt.net"), nettpn("Data\\tpn.net")
{
	m_formation.ReadFormation("Data\\Strategy.nse");
	
//	m_svmModelTpt = svm_load_model("Data\\tpt.model");
//	m_svmAttrTpt = (struct svm_node *) malloc(4*sizeof(struct svm_node));
		
	m_timerStart.AttachListener(this);
	m_timerShootLine.AttachListener(this);
	m_timerBallKick.AttachListener(this);

	Initialize();//初始化函数
}

CDecisionMakingx::~CDecisionMakingx()
{
//	svm_destroy_model(m_svmModelTpt);
//	free(m_svmAttrTpt);
}

/////////////// Basic Actions ///////////////////////////////////////////////////////
/************************************************************************/
/* 避障到定点                                                           */
/************************************************************************/
void CDecisionMakingx::ToPositionAvoidObstacles(int nRobot, VecPosition pos[], 
							  int nObstacles)
{
	dbLRWheelVelocity* pSpeed = &rbV[nRobot];

	// Hundun算法输入参数,q[0]=当前点，q[1]=目标点，其他障碍物点
	VecPosition* q = new VecPosition[nObstacles+2];

	q[0].SetVecPosition(Robot[nRobot].x, Robot[nRobot].y);
	q[1] = pos[0];
	
	for (int i=0; i<nObstacles; i++)
		q[i+2] = pos[i+1];

	double theta = Robot[nRobot].theta;

	// 转变坐标系，Hundun算法目前要求目标点在起始点右上
	BOOL bRight = TRUE, bTop=TRUE;
	// 左右转换
	if (q[1].GetX() < q[0].GetX())
	{
		bRight = FALSE;
		for (int i=0; i<nObstacles+2; i++)
			q[i].SetX(PITCH_LENGTH - q[i].GetX());

		theta = VecPosition::NormalizeAngle(PI-theta);
	}
	// 上下转换
	if (q[1].GetY() < q[0].GetY())
	{
		bTop = FALSE;
		for (int i=0; i<nObstacles+2; i++)
			q[i].SetY(PITCH_WIDTH - q[i].GetY());

		theta = VecPosition::NormalizeAngle(-theta);
	}

	m_hundun.hundun(q, nObstacles);
	
	double dTheta = m_hundun.X[1] - theta;

	double w = dTheta;
	double v =  (7-nObstacles)*m_hundun.X[0];
	double L = 4;
	double VL = v - 10*w*L;
	double VR = v + 10*w*L;

	pSpeed->LeftValue = VL;
	pSpeed->RightValue = VR;

	if (!bRight)
	{
		double temp = pSpeed->LeftValue;
		pSpeed->LeftValue = pSpeed->RightValue;
		pSpeed->RightValue = temp;
	}
	if (!bTop)
	{
		double temp = pSpeed->LeftValue;
		pSpeed->LeftValue = pSpeed->RightValue;
		pSpeed->RightValue = temp;
	}

//	LimitSpeed(&rbV[1], 110, TRUE);
	
	if ( sqrt((m_hundun.XYTO[0] - m_hundun.xg)*(m_hundun.XYTO[0] - m_hundun.xg)
		+ (m_hundun.XYTO[1] - m_hundun.yg)*(m_hundun.XYTO[1] - m_hundun.yg))
		< 3)
	{
		pSpeed->LeftValue =0;
		pSpeed->RightValue = 0;
	}

	delete[] q;
}


/************************************************************************/
/* 本函数用于使小车的方向以给定的时钟方向快速转到所要求的角度方向		*/
/*  可调参数 ：Kp，Kd为比例、微分调节, angle角度误差					*/
/************************************************************************/
void CDecisionMakingx::TurnToAnglePD(int nRobot,double dAngle,int clock)
{
	dbLRWheelVelocity *pSpeed = &rbV[nRobot];

	static double anglePrev[ROBOTNUMBER] = {0};

	double angle,dSameSpeed;
	int nQuadrant = 0;
	angle = Robot[nRobot].theta - dAngle;
    angle =  VecPosition::NormalizeAngle(angle);
	if (angle < 0)
		angle += 2*PI;

	if (clock == ANTICLOCK)
	{
		angle = 2*PI - angle;
	}
	else if (clock == NOCLOCK)
	{
		angle =  VecPosition::NormalizeAngle(angle);

		//判断角度差所在象限
		if (angle >= -PI && angle < -PI/2)
		{
			nQuadrant = 3;
			angle = angle+PI;
		}
		else if (angle >= -PI/2 && angle < 0)
		{
			nQuadrant = 4;
			angle = -angle;
		}
		else if (angle >= 0 &&  angle < PI/2)
		{
			nQuadrant=1;
		}
		else if (angle >= PI/2 &&  angle <= PI)
		{
			nQuadrant = 2;
			angle = PI-angle;
		}
	}

#ifdef SimuroSot5 
	double angleMax = 2.0/180.0*PI;
	//此处进行PD调节
	double Kp = 10;
	double Kd = 50;
#endif
	
#ifdef SimuroSot11
	double angleMax = 2.0/180.0*PI;
	double Kp = 10;
	double Kd = 50;
#endif

	if (fabs(angle) <= angleMax)//判断是否在角度误差限之内
	{
		pSpeed->LeftValue = 0;
		pSpeed->RightValue = 0;
		return;
	}

    dSameSpeed = Kp*angle + Kd*(angle - anglePrev[nRobot]);
	dSameSpeed = Maths::Limit(dSameSpeed, 0, 40);

	anglePrev[nRobot] = angle;

	if (clock==CLOCKWISE)
	{
		pSpeed->LeftValue = dSameSpeed;
		pSpeed->RightValue = -dSameSpeed;
	}
	else if (clock == ANTICLOCK)
	{
		pSpeed->LeftValue = -dSameSpeed;
		pSpeed->RightValue = dSameSpeed;
	}
	else 
	{
		switch(nQuadrant)
		{
			case 1://顺时针旋转 
			case 3:
			{
				pSpeed->LeftValue = dSameSpeed;
				pSpeed->RightValue = -dSameSpeed;
				break;                     
			}
			case 2://逆时针旋转
			case 4:
			{
				pSpeed->LeftValue = -dSameSpeed;
				pSpeed->RightValue = dSameSpeed;
				break;
			}
		}
	}	
}

/************************************************************************/
/*	本函数用于使小车快速转向指定点										*/
/*	Robot为小车位置信息，Point为转向的点，pSpeed为返回的左右轮速		*/
/*	Kp、Kd为比例、微分调节参数											*/
/************************************************************************/
void CDecisionMakingx::TurnToPointPD(int nRobot, VecPosition posTarget, int clock)	
{
	double dAngle = (posTarget - VecPosition(Robot[nRobot].x, Robot[nRobot].y)).GetDirection();
	TurnToAnglePD(nRobot,dAngle,clock);
}

/************************************************************************/
/*	本函数用于定点旋转，成功返回1，										*/
/*	Velocity为要求旋转的角速度，Velocity为正，按逆时针方向旋转，为负，	*/
/*  按顺时针方向旋转，pSpeed为返回的左右轮速							*/
/************************************************************************/
void CDecisionMakingx::Turn(int nRobot, double dVelocity,int clock)
{
	dbLRWheelVelocity *pSpeed = &rbV[nRobot];
	
	if (clock == CLOCKWISE)
	{
		pSpeed->LeftValue = dVelocity;
		pSpeed->RightValue = -dVelocity;
	}
	else 
	{
		pSpeed->LeftValue = -dVelocity;
		pSpeed->RightValue = dVelocity;
	}
}

/************************************************************************/
/*  本函数用于小车沿某一角度方向移动,                                   */
/*  采用余弦算法
/************************************************************************/
void CDecisionMakingx::MoveOnAngle(int nRobot,double Angle,double speed)
{
	dbLRWheelVelocity* pSpeed = &rbV[nRobot];

	double dDifference;
	dDifference = Robot[nRobot].theta - Angle;
	dDifference = VecPosition::NormalizeAngle(dDifference);

	if (dDifference >= -PI && dDifference < -PI/2)
	{
		pSpeed->RightValue = -speed;
		pSpeed->LeftValue = pSpeed->RightValue*cos(dDifference+PI);
	}
	else if (dDifference >= -PI/2 && dDifference < 0)
	{
		pSpeed->RightValue=speed;
		pSpeed->LeftValue=pSpeed->RightValue*cos(-dDifference);
	}
	else if (dDifference >= 0 && dDifference < PI/2)
	{
		pSpeed->LeftValue = speed;
		pSpeed->RightValue  =pSpeed->LeftValue*cos(dDifference);
	}
	else if (dDifference >= PI/2 && dDifference < PI)
	{
		pSpeed->LeftValue=-speed;
		pSpeed->RightValue=pSpeed->LeftValue*cos(PI-dDifference);
	}
}

/************************************************************************/
/* 以余弦曲线且按给定的速度到定点                                       */
/* 可调参数：Kp, 余弦系数
/************************************************************************/
void CDecisionMakingx::MoveToPt(int nRobot,VecPosition posTarget,double speed)
{
	dbLRWheelVelocity* pSpeed = &rbV[nRobot];

	double theta,temp_theta;
	double dx,dy,dx1,dy1;
	double distance;

	//坐标变换，先平移后转角，以小车中心为原点，小车朝向为y轴
	dx1 = posTarget.GetX() - Robot[nRobot].x;
	dy1 = posTarget.GetY() - Robot[nRobot].y;
	dx = dx1*cos(Robot[nRobot].theta-PI/2) + dy1*sin(Robot[nRobot].theta-PI/2);
	dy = -dx1*sin(Robot[nRobot].theta-PI/2) + dy1*cos(Robot[nRobot].theta-PI/2);

	distance = sqrt(dx*dx+dy*dy);
	theta = atan2(dy,dx);	

	double Kp = 0.8;

	if (theta >= -PI && theta < -PI/2)
	{
		temp_theta = -PI/2 - theta;
		if(fabs(temp_theta) < PI/7) temp_theta *= 2;
		pSpeed->LeftValue = -cos(temp_theta)*Kp*speed;
		pSpeed->RightValue = -speed;
	}
	else if (theta >= -PI/2 && theta < 0)
	{
		temp_theta = theta + PI/2;
		if(fabs(temp_theta) < PI/7) temp_theta *= 2;
		pSpeed->LeftValue = -speed;
		pSpeed->RightValue = -cos(temp_theta)*Kp*speed;
	}
	else if(theta >= 0 && theta < PI/2)//第一象限
	{
		temp_theta = PI/2 - theta;
		if(temp_theta < PI/7) temp_theta *= 2;
		pSpeed->LeftValue = speed;
		pSpeed->RightValue = cos(temp_theta)*Kp*speed;
	}
	else if(theta >= PI/2 && theta < PI)//第二象限
	{
		temp_theta = theta - PI/2;
		if(fabs(temp_theta) < PI/7) temp_theta *= 2;
		pSpeed->LeftValue = cos(temp_theta)*Kp*speed;
		pSpeed->RightValue = speed;
	}
}

/************************************************************************/
/*  到定点,PD算法                                                       */
/*  可调参数：Kp,Kd:角速度w的PD系数	;angle:当角度偏差大于angle时，先转	*/
/************************************************************************/
void CDecisionMakingx::ToPositionPD(int nRobot, VecPosition posTarget,
									double speed)
{          
	VecPosition posRobot(Robot[nRobot].x, Robot[nRobot].y);
	dbLRWheelVelocity* pSpeed = &rbV[nRobot];

	//计算角度偏差
	static double distPrev[ROBOTNUMBER] = {0};
	static double anglePrev[ROBOTNUMBER] = {0};

	double dist = (posRobot - posTarget).GetMagnitude();
	double angle = VecPosition::NormalizeAngle(
		(posTarget - posRobot).GetDirection() - Robot[nRobot].theta);


#ifdef SimuroSot5 
	double angleMax = PI*0.48;	// 大于此角度先偏转
	double kpa = 100, kda = 0;
	double kp = 50, kd = 300;
#endif

#ifdef SimuroSot11
	double angleMax = PI*0.48;	// 大于此角度先偏转
	double kpa = 100, kda = 0;
	double kp = 50, kd = 300;
#endif

	
	double speed_e;

	int nQuadrand = -1;
	if(angle > -PI && angle < -PI/2)
	{
		nQuadrand = 3;
		angle = angle + PI;
	}
	else if (angle > -PI/2 && angle < 0)
	{
		nQuadrand = 4;
		angle = -angle;
	}
	else if (angle > 0 && angle <= PI/2)
	{
		nQuadrand = 1;
		angle = angle;
	}
	else if(angle > PI/2 && angle <= PI)
	{
		nQuadrand = 2;
		angle = PI - angle;
	}
	if (dist < 0.2)
		dist = 0;
	if (angle < PI/90)
		angle = 0;