decisionmakingx.cpp

robocup 5vs5修改板决策例程

// DecisionMakingx.cpp: implementation of the CDecisionMakingx class.
//
//////////////////////////////////////////////////////////////////////

#include "stdafx.h"
#include "DecisionMakingx.h"
#include <fstream>
#include <algorithm>
#include <set>
#include <cassert>
#include <math.h>

CDecisionMakingx::CDecisionMakingx() 
{
	int i=0;
	for (i=0;i<5;i++)
	{
		rbV[i].LeftValue = 0;
		rbV[i].RightValue = 0;
	}
}

CDecisionMakingx::~CDecisionMakingx()
{
	
}


/************************************************************************/
/* 决策入口                                                             */
/************************************************************************/
void CDecisionMakingx::Startx(RobotInford	dmRobot[])
{	
	//决策使用Robot[]数组表示机器人的位置、方向
	for(int i=0; i<2*ROBOTNUMBER+1; ++i)
	{
		Robot[i].pos = dmRobot[i].pos;
		Robot[i].theta = dmRobot[i].theta;
		Robot[i].theta = VecPosition::NormalizeAngle(Robot[i].theta);
	}
	PreProcess();//信息预处理
	
	MiroSot_DecisionMaking();	

	PostPrecess();//信息后处理
}

/************************************************************************/
/*  MIROSOT比赛入口                                                     */
/************************************************************************/
void CDecisionMakingx::MiroSot_DecisionMaking()
{	
	GoalieActionExample(4,100);//守门员函数,4号车作守门员
	
	VecPosition posTarget1(GOAL_AREA_LENGTH+10,m_posBall.GetY());
	ToPositionPD(1, posTarget1, 50);//1号车作第二守门员


	int m_nAreaNo= GetAreaNoExample(m_posBall);//得到区域号码

	switch(m_nAreaNo)
	{
	case 1://进攻型阵形
		{    
			VecPosition posTarget21(PITCH_LENGTH*3 / 4+10,PITCH_WIDTH/2+40);
			ToPositionPD(2, posTarget21, 60);
			VecPosition posTarget31(PITCH_LENGTH*3 / 4+10,PITCH_WIDTH/2-40);
			ToPositionPD(3, posTarget31, 60);
			Vect_MidShoot(0);
		}
		break;

	case 2://站位型阵形
		{   
			VecPosition posTarget22(PITCH_LENGTH / 2-20,PITCH_WIDTH/2+40); 
			ToPositionPD(2, posTarget22, 60);
			VecPosition posTarget32(PITCH_LENGTH / 2-20,PITCH_WIDTH/2-40);
			ToPositionPD(3, posTarget32, 60);
			Vect_MidShoot(0);
		}
		break;

	case 3://防守阵形
		{ 
			VecPosition posTarget23(GOAL_AREA_LENGTH+10,m_posBall.GetY()+15);
			ToPositionPD(2, posTarget23, 60);
			VecPosition posTarget33(PITCH_LENGTH / 4,m_posBall.GetY());
			ToPositionPD(3, posTarget33, 60);
			Vect_MidShoot(0);
		}
		break;
	}
return;
}

/************************************************************************/
/*  到定点,PD算法                                                       */
/*  可调参数：Kp,Kd:角速度w的PD系数	;angle:当角度偏差大于angle时，先转	*/
/************************************************************************/
void CDecisionMakingx::ToPositionPD(int nRobot, VecPosition posTarget, double speed)
{  
	static double e1[ROBOTNUMBER] = {0};
	static double d1[ROBOTNUMBER] = {0};
	static VecPosition posRobot1[ROBOTNUMBER], posTarget1[ROBOTNUMBER];
	static double dErrorCumu[ROBOTNUMBER]= {0}; 

	VecPosition posRobot(Robot[nRobot].pos);
	double theta = Robot[nRobot].theta;
	dbLRWheelVelocity* pSpeed = &rbV[nRobot];

	// When the Targetpoint changes,SET the error 0.
	if (posRobot1[nRobot].GetDistanceTo(posRobot) > 10 
		|| posTarget1[nRobot].GetDistanceTo(posTarget) > 10)
	{
		e1[nRobot] = 0;
		d1[nRobot] = 0;
		dErrorCumu[nRobot] = 0;
	}

	VecPosition posDelta = posTarget - posRobot;
	double dist = posDelta.GetMagnitude();
	double e =posDelta.GetDirection()-theta ;
	e = VecPosition::NormalizeAngle(e);

	BOOL bBackward = FALSE;
	if (fabs(e) > PI/2)
	{
		bBackward = TRUE;
		e = (PI - fabs(e))*Maths::Sign(e);
	}

	double w;
	double kp, kd;
	kp =2.22; kd = 3.28;//kp,kd can be adjusted,if the robot is NOT controlled well.
	w = kp*e + kd*(e-e1[nRobot]);

	// Integral Separation Control Algorithm.
	double d = (posDelta.GetX()*cos(theta) + posDelta.GetY()*sin(theta));
	double kp1, kd1, ki1;
	kp1 =2.5, ki1 = 0.0, kd1 = 7.8;//kp,kd can be adjusted,if the robot is NOT controlled well.

	if (fabs(d) < 20)
	{
		ki1 =0.12;//ki can be adjusted,if the robot is NOT controlled well.
		dErrorCumu[nRobot] = dErrorCumu[nRobot] + d;
	}

	double v = kp1*d + kd1*(d-d1[nRobot]);

	if (fabs(v) > speed)
		v = speed * Maths::Sign(v);

	v = v * exp(-1*1.5*fabs(e));

	// When the robot is close to TargetPoint,SET the speed 0 in order to make the robot steady.
	if (dist < 3)
	{
		v = 0;
		w = 0;
	}

	e1[nRobot] = e;
	d1[nRobot] = d;
	posRobot1[nRobot] = posRobot; posTarget1[nRobot] = posTarget;

	if (bBackward)
	{
		w = -w;                              
	}

	pSpeed->LeftValue = (v - ROBOT_LENGTH*w);
	pSpeed->RightValue =(v + ROBOT_LENGTH*w);
	LimitSpeed(pSpeed,120, FALSE);//120 is the maximum value which is used for limited speed in case that the robot is not under steady condition.
	//It can be modified discretionarily on the basis of making the robot steady.
}
/************************************************************************/
/* 本函数用于使小车的方向以给定的时钟方向快速转到所要求的角度方向		*/
/*  可调参数 ：Kp，Kd为比例、微分调节, angle角度误差					*/
/************************************************************************/
void CDecisionMakingx::TurnToAnglePD(int nRobot,double dAngle)
{
	static double e1[ROBOTNUMBER] = {0};

	VecPosition posRobot(Robot[nRobot].pos);
	double theta = Robot[nRobot].theta;
	dbLRWheelVelocity* pSpeed = &rbV[nRobot];

	double e = dAngle- theta ;
	e = VecPosition::NormalizeAngle(e);

	// 方向:应该backward,但dist也有+-,所以只需w变方向即可
	BOOL bBackward = FALSE;
	if (fabs(e) > PI/2)
	{
		bBackward = TRUE;
		e = (PI - fabs(e))*Maths::Sign(e);
	}

	double w;
	double kp, kd;
	kp = 3.5; kd =0.5;
	w = kp*e + kd*(e-e1[nRobot]);

	e1[nRobot] = e;

	if (bBackward)
	{
		w = -w;                              
	}

	pSpeed->LeftValue =  -ROBOT_LENGTH*w;
	pSpeed->RightValue = ROBOT_LENGTH*w;

	LimitSpeed(pSpeed, 1, FALSE);
}
/************************************************************************/
/*  中分线射门                                                          */
/************************************************************************/
void CDecisionMakingx::Vect_MidShoot(int nRobot)
{
	VecPosition posRobot(Robot[nRobot].pos);
	VecPosition posGoal(PITCH_LENGTH, PITCH_WIDTH/2);
	VecPosition posBall = m_posBall;

	Line lineBall2Goal = Line::MakeLineFromTwoPoints(posGoal, posBall);
	double L = 0, R = 30;
	VecPosition posShoot = lineBall2Goal.GetPointInLine(posBall, -L);
	Line linePerp = lineBall2Goal.GetPerpendicularLine(posShoot);
	VecPosition posCenter = linePerp.GetPointInLine(posShoot, -R);
	Circle c(posCenter, R);
	Line lineRobot2Shoot = Line::MakeLineFromTwoPoints(posRobot, posShoot);
	VecPosition posMid = (posRobot + posShoot)/2;
	Line linePerp2 = lineRobot2Shoot.GetPerpendicularLine(posMid);
	VecPosition posSolu[2], posTarget;
	linePerp2.GetCircleIntersectionPoints(c, &posSolu[0], &posSolu[1]);
	if ( posSolu[0].GetDistanceTo(posRobot) > posSolu[1].GetDistanceTo(posRobot))
		posTarget = posSolu[1];
	else
		posTarget = posSolu[0];

	ToPositionPD(nRobot, posTarget, 100);
	if (posRobot.GetX()<posBall.GetX()
		&&fabs(posRobot.GetY()-posBall.GetY())<5 ) 
	{
       ToPositionPD(nRobot, posGoal, 100);
	}
	
	//SendMsg(3, "%2.2f, %2.2f", posTarget.GetX(), posTarget.GetY());
}


/************************************************************************/
/*  传递比赛开球模式,及何方开球。详见文档：相关信息     		        */
/************************************************************************/
void CDecisionMakingx::InitDEG(DEGame DEG)
{
	dmDEG = DEG;
}

/************************************************************************/
/* 限速(最大dMadSpeed)                                                  */
/* bMax = TRUE -> 保证角速度,提高线速度									*/
/************************************************************************/
void CDecisionMakingx::LimitSpeed(dbLRWheelVelocity *pSpeed, double dMaxSpeed, BOOL bMax)
{
	double speed_e = pSpeed->LeftValue - pSpeed->RightValue;
	
	if (speed_e > 2*dMaxSpeed)
		speed_e = 2*dMaxSpeed;

	if (fabs(pSpeed->LeftValue) > fabs(pSpeed->RightValue))
	{
		if (pSpeed->LeftValue > dMaxSpeed)
		{
			pSpeed->LeftValue = dMaxSpeed;
			pSpeed->RightValue = pSpeed->LeftValue - speed_e;
		}
		else if (pSpeed->LeftValue < -dMaxSpeed)
		{
			pSpeed->LeftValue = -dMaxSpeed;
			pSpeed->RightValue = pSpeed->LeftValue - speed_e;
		}
		
	}
	else
	{
		if (pSpeed->RightValue > dMaxSpeed)
		{
			pSpeed->RightValue = dMaxSpeed;
			pSpeed->LeftValue = pSpeed->RightValue + speed_e;
		}
		else if (pSpeed->RightValue < -dMaxSpeed)
		{
			pSpeed->RightValue = -dMaxSpeed;
			pSpeed->LeftValue = pSpeed->RightValue + speed_e;
		}
	}

	if (bMax)
	{
		if (fabs(pSpeed->LeftValue) > fabs(pSpeed->RightValue))
		{
			if (pSpeed->LeftValue > 0)
				pSpeed->LeftValue = dMaxSpeed;
			else 
				pSpeed->LeftValue = -dMaxSpeed;

			pSpeed->RightValue = pSpeed->LeftValue - speed_e;
		}
		else
		{
			if (pSpeed->RightValue > 0)
				pSpeed->RightValue = dMaxSpeed;
			else 
				pSpeed->RightValue = -dMaxSpeed;

			pSpeed->LeftValue = pSpeed->RightValue + speed_e;
		}
	}
}

/************************************************************************/
/*  基于球的位置的分区法得到区域号码                                    */
/************************************************************************/
int CDecisionMakingx::GetAreaNoExample(VecPosition posBall)
{  
   int nAreaNo = 0;
  if (posBall.GetX()>PITCH_LENGTH*3 / 4) 
  {
    nAreaNo = 1;
  } 
  if(posBall.GetX()>=PITCH_LENGTH / 2&&posBall.GetX()<=PITCH_LENGTH*3 / 4)
  {
    nAreaNo = 2;
  }
  if(posBall.GetX()<PITCH_LENGTH / 2)
  {
    nAreaNo = 3;
  }
 return nAreaNo;	
}
/************************************************************************/
/*  简单守门员函数                                                      */
/************************************************************************/
void CDecisionMakingx::GoalieActionExample(int nRobot,double speed)
{
	if (m_posBall.GetY()>PITCH_WIDTH/2)

	{
      VecPosition posTargetGoalie1(10,PITCH_WIDTH/2 + GOAL_WIDTH/2);
      ToPositionPD(nRobot, posTargetGoalie1, speed);
	}
	else
	{
      VecPosition posTargetGoalie2(10,PITCH_WIDTH/2 - GOAL_WIDTH/2);
      ToPositionPD(nRobot, posTargetGoalie2, speed);
	}
}


/************************************************************************/
/*  信息预处理函数                                                      */
/************************************************************************/
void CDecisionMakingx::PreProcess()
{
	//左右半场转换
	if(dmDEG.DEGameGround == RightArea)	
	{	
		for(int i=0; i<2*ROBOTNUMBER+1; i++)
		{
			Robot[i].pos = VecPosition(PITCH_LENGTH, PITCH_WIDTH) - Robot[i].pos;
			Robot[i].theta = Robot[i].theta + PI;
			if(Robot[i].theta > 2*PI) Robot[i].theta -= 2*PI;
			if(Robot[i].theta > PI) Robot[i].theta -= 2*PI;
		}
	}
	//处理球
	m_posBall = Robot[2*ROBOTNUMBER].pos;
}


/************************************************************************/
/*  信息后处理函数，保存本周期机器人的位置方向信息（本版本例程没有用到）*/
/************************************************************************/
void CDecisionMakingx::PostPrecess()
{
	for (int i=0; i<ROBOTNUMBER; i++)
	{
		oldRobot[i] = Robot[i];
		
		oldrbV[i] = rbV[i];
	}
}