decisionmakingx.cpp

该程序实现FIRE足球机器人竞赛中的3：3比赛源码

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

#include "stdafx.h"
#include "DecisionMakingx.h"

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

CDecisionMakingx::CDecisionMakingx()
{
	Initialize();//初始化函数
}

CDecisionMakingx::~CDecisionMakingx()
{

}
//根据给定的半径和同步速度得到左右轮速，radius以运动方向的法线方向为正,且不能溢出
int  CDecisionMakingx::Circle(double Radius,double SameSpeed,dbLRWheelVelocity* pLRWheelVelocity)
{
	if(Radius == 0)
	{
		pLRWheelVelocity->LeftValue = -SameSpeed;
		pLRWheelVelocity->RightValue = SameSpeed;
	}
	else
	{	
		if(SameSpeed>0)
		{	
			//半径为正时左轮小，为负左轮大		
			pLRWheelVelocity->LeftValue = (Radius-4)/Radius*SameSpeed;
			pLRWheelVelocity->RightValue = (Radius+4)/Radius*SameSpeed;			
		}
		else
		{
			//半径为正时左轮负大，为负左轮负小		
			pLRWheelVelocity->LeftValue =  (Radius+4)/Radius*SameSpeed;
			pLRWheelVelocity->RightValue =  (Radius-4)/Radius*SameSpeed;			
		}
	}
	return 0;
}
//给定车和目标点，求出车以给定的速度以一定的时针方向经过该点的圆的轨迹,clockwise=1顺时针，=-1逆时针
int  CDecisionMakingx::CirclePassTarget(dbROBOTPOSTURE* pROBOTPOSTURE,dbPOINT Target,double SameSpeed,int Clockwise,dbLRWheelVelocity* pLRWheelVelocity)
{
	double radius;
	double speed;
	double dx,dy;
	dbPOINT temp;

	//坐标变换，以小车中新为原点，小车朝向为y轴
	dx = Target.x - pROBOTPOSTURE->x;
	dy = Target.y - pROBOTPOSTURE->y;
	temp.x = dx*cos(pROBOTPOSTURE->theta - pi/2) + dy*sin(pROBOTPOSTURE->theta - pi/2);
	temp.y = -dx*sin(pROBOTPOSTURE->theta - pi/2) + dy*cos(pROBOTPOSTURE->theta - pi/2);
	
	if(fabs(temp.x) <= 0.001)//在新坐标系的y轴上
	{
		if(temp.y>0)//上半轴
		{
			pLRWheelVelocity->LeftValue = SameSpeed;
			pLRWheelVelocity->RightValue = SameSpeed;
		}
		else//下半轴
		{
			pLRWheelVelocity->LeftValue = -SameSpeed;
			pLRWheelVelocity->RightValue = -SameSpeed;
		}
		return 0;
	}
	if(Clockwise == CLOCKWISE)//顺时针
	{	
		speed = SameSpeed;		
		if(temp.x > 0)//在新坐标系的第一、四象限
			radius = -SolveRadius(temp);
		else //在新坐标系的第二、三象限
			radius = SolveRadius(temp);
	}
	else if(Clockwise == ANTICLOCK)//逆时针
	{
		speed = -SameSpeed;		
		if(temp.x > 0)//在新坐标系的第一、四象限
			radius = SolveRadius(temp);
		else //在新坐标系的第二、三象限
			radius = -SolveRadius(temp);
	}
	Circle(radius,speed,pLRWheelVelocity);
	return 1;
}
int CDecisionMakingx::TurnToAnglePD(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;
		pSpeed->RightValue=-SameSpeed;
	}
	else if (clock == ANTICLOCK)
	{
		pSpeed->LeftValue=-SameSpeed;
		pSpeed->RightValue=SameSpeed;
	}
	else 
	{
		switch(Quadrant)
		{
			case 1://顺时针旋转 
			case 3:
			{
				pSpeed->LeftValue=SameSpeed;
				pSpeed->RightValue=-SameSpeed;
				break;                     
			}
			case 2://逆时针旋转
			case 4:
			{
				pSpeed->LeftValue=-SameSpeed;
				pSpeed->RightValue=SameSpeed;
				break;
			}
		}
	}	
	return 1;
}
int CDecisionMakingx::s_TurnToAnglePD(dbROBOTPOSTURE *pRobot,double dbAngle,dbLRWheelVelocity *pSpeed)
//本函数用于使小车快速转向指定点
//Robot为小车位置信息，Point为转向的点，pSpeed为返回的左右轮速
//Kp、Kd为比例、微分调节参数
{
	double theta,theta1;
	double SameSpeed;
	dbAngle = cn_AngleTrim2PI(dbAngle);
	pRobot->theta = cn_AngleTrim2PI(pRobot->theta);
	theta = pRobot->theta - dbAngle;
	if(theta < -pi) 
		theta = 2*pi + theta;
	else if(theta > pi) 
		theta =  theta - 2*pi;
	theta1 = fabs(theta);
	if (theta1 < m_AngleParameter.AngleError)//判断是否在角度误差限之内
	{
		pSpeed->LeftValue = 0;
		pSpeed->RightValue = 0;
		return 1;
	}
    SameSpeed = m_AngleParameter.Kp*theta1 + m_AngleParameter.Kd*(theta1 - m_Front);
	if (SameSpeed > m_AngleParameter.MaxAngleSpeed)
		SameSpeed = m_AngleParameter.MaxAngleSpeed;
	m_Front = theta1;
	if(theta < 0)
	{
		pSpeed->LeftValue = -SameSpeed;
		pSpeed->RightValue = SameSpeed;
		
	}
	else
	{
		pSpeed->LeftValue = SameSpeed;
		pSpeed->RightValue = -SameSpeed;	
	}
	return 1;
}
/************************************************************************************
函数名	：TurnTo(dbROBOTPOSTURE* robot, ballInformation ball, dbPOINT target,int wNo,int Vbase,dbLRWheelVelocity *pSpeed)
创建日期：19/4/2001
作者	：
功能	：转身拨球
参数	：	robot				机器人位姿
			target				目标点
			ball				球的位置
			wNo					拨球方式
			Vbase				基准速度
************************************************************************/
int CDecisionMakingx::TurnTo(dbROBOTPOSTURE* robot, ballInformation ball, dbPOINT target,/*int wNo,*/int Vbase,dbLRWheelVelocity *pSpeed)
{
	double	robotL = 7.5;
	double theta;
	robot->y = walltop - robot->y;
	robot->theta = 2*pi - robot->theta;
	ball.y = walltop - ball.y;
	target.y = walltop - target.y;
	theta = cn_AngleTrim2PI(robot->theta);
	dbPOINT pt[5];
	//求机器人的四个顶点
	pt[1].x = robot->x + (sqrt(2)*cos(theta - pi/4)/2)*robotL;
	pt[1].y = robot->y + (sqrt(2)*sin(theta - pi/4)/2)*robotL;
	pt[2].x = robot->x + (sqrt(2)*cos(theta + pi/4)/2)*robotL;
	pt[2].y = robot->y + (sqrt(2)*sin(theta + pi/4)/2)*robotL;
	pt[3].x = robot->x + (sqrt(2)*cos(theta + 3*pi/4)/2)*robotL;
	pt[3].y = robot->y + (sqrt(2)*sin(theta + 3*pi/4)/2)*robotL;
	pt[4].x = robot->x + (sqrt(2)*cos(theta - 3*pi/4)/2)*robotL;
	pt[4].y = robot->y + (sqrt(2)*sin(theta - 3*pi/4)/2)*robotL;
	//四个顶点和球的距离
	double distpt2ball[5];
	distpt2ball[1] = (pt[1].x-ball.x)*(pt[1].x-ball.x) + (pt[1].y-ball.y)*(pt[1].y-ball.y);
	distpt2ball[2] = (pt[2].x-ball.x)*(pt[2].x-ball.x) + (pt[2].y-ball.y)*(pt[2].y-ball.y);
	distpt2ball[3] = (pt[3].x-ball.x)*(pt[3].x-ball.x) + (pt[3].y-ball.y)*(pt[3].y-ball.y);
	distpt2ball[4] = (pt[4].x-ball.x)*(pt[4].x-ball.x) + (pt[4].y-ball.y)*(pt[4].y-ball.y);
	//找到距离球最近的两个顶点
	int ptNo[5];
	for(int i=0;i<=5;i++)
		ptNo[i]=i;
	double temp;
	int tempi;
	for(i=0;i<3;i++)
	{
		for(int j=1;j<=3-i;j++)
		{
			if(distpt2ball[j] < distpt2ball[j+1])
			{
				temp = distpt2ball[j];
				tempi = ptNo[j];
				distpt2ball[j] = distpt2ball[j+1];
				ptNo[j] = ptNo[j+1];
				distpt2ball[j+1] = temp;
				ptNo[j+1] = tempi;
			}
		}
	}
	//找到离球近的两个点中距离目标点远的那个点
	double distpt2target1,distpt2target2;
	distpt2target1 = (pt[ptNo[3]].x-target.x)*(pt[ptNo[3]].x-target.x) + (pt[ptNo[3]].y-target.y)*(pt[ptNo[3]].y-target.y);
	distpt2target2 = (pt[ptNo[4]].x-target.x)*(pt[ptNo[4]].x-target.x) + (pt[ptNo[4]].y-target.y)*(pt[ptNo[4]].y-target.y);
	if(distpt2target1 > distpt2target2)
	{
		tempi = ptNo[3];
		ptNo[3] = ptNo[4];
		ptNo[4] = tempi;
	}
	if(ptNo[3] < ptNo[4] && ptNo[3] != 4 && ptNo[4] != 1 || ptNo[3]==1 && ptNo[4] == 4)
	{
		pSpeed->LeftValue =- Vbase;
		pSpeed->RightValue = Vbase;
	}
	else
	{
		pSpeed->LeftValue = Vbase;
		pSpeed->RightValue =- Vbase;
	}
	return 1;
}
int CDecisionMakingx::s_TurnToPointPD(dbROBOTPOSTURE *pRobot,dbPOINT Point,dbLRWheelVelocity *pSpeed)	
//本函数用于使小车快速转向指定点
//Robot为小车位置信息，Point为转向的点，pSpeed为返回的左右轮速
//Kp、Kd为比例、微分调节参数
{
	double dx1,dy1,dx,dy;
	double theta,theta1;
	double SameSpeed;
	//坐标变换，以小车中心为原点，小车朝向为y轴
	dx1 = Point.x - pRobot->x;
	dy1 = Point.y - pRobot->y;
	dx = dx1*cos(pRobot->theta - pi/2) + dy1*sin(pRobot->theta - pi/2);
	dy = -dx1*sin(pRobot->theta - pi/2) + dy1*cos(pRobot->theta - pi/2);
	theta = atan2(dy,dx);
	if(theta < 0) theta = pi/2 - theta;
	else theta = pi/2 - theta;
	if(theta > pi) theta = theta - 2*pi;
	theta1 = fabs(theta);
	if (theta1 < m_AngleParameter.AngleError)//判断是否在角度误差限之内
	{
		pSpeed->LeftValue = 0.;
		pSpeed->RightValue = 0.;
		return 1;
	}
    SameSpeed = m_AngleParameter.Kp*theta1 + m_AngleParameter.Kd*(theta1 - m_Front);
	if (SameSpeed > m_AngleParameter.MaxAngleSpeed)
		SameSpeed = m_AngleParameter.MaxAngleSpeed;
	m_Front = theta1;
	if(theta < 0)
	{
		pSpeed->LeftValue = -SameSpeed;
		pSpeed->RightValue = SameSpeed;
	}
	else
	{
		pSpeed->LeftValue = SameSpeed;
		pSpeed->RightValue = -SameSpeed;
	}
	return 1;
}
int CDecisionMakingx::TurnToPointPD(dbROBOTPOSTURE *pRobot,dbPOINT Point,int clock,dbLRWheelVelocity *pSpeed)	
//本函数用于使小车快速转向指定点
//Robot为小车位置信息，Point为转向的点，pSpeed为返回的左右轮速
//Kp、Kd为比例、微分调节参数
{
	double Angle;
	int Result;
	dbPOINT Point1;
	Point1.x=pRobot->x;
	Point1.y=pRobot->y;
	Result=PointToPointDirectionAngle(Point1,Point,&Angle);
	if (Result==0)
		return 0;
	Result=TurnToAnglePD(pRobot,Angle,clock,pSpeed);
	return (Result);
}
int CDecisionMakingx::Turn(double dbVelocity,int clock,dbLRWheelVelocity *pSpeed)
//本函数用于定点旋转，成功返回1，
//Velocity为要求旋转的角速度，Velocity为正，按逆时针方向旋转，为负，
//按顺时针方向旋转，pSpeed为返回的左右轮速
{
	if (clock == CLOCKWISE)
	{
		pSpeed->LeftValue=dbVelocity;
		pSpeed->RightValue=-dbVelocity;
	}
	else 
	{
		pSpeed->LeftValue=-dbVelocity;
		pSpeed->RightValue=dbVelocity;
	}
	return 1;
}
int CDecisionMakingx::MoveOnAngle(dbROBOTPOSTURE *pRobot,double Angle,double speed,dbLRWheelVelocity *pSpeed)
//本函数用于小车沿某一角度方向移动,
//
{
	double Difference;
	Difference=pRobot->theta-Angle;
	Difference = cn_AngleTrim2PI(Difference);
	if (Difference >= 0 && Difference < pi/2)
	{
		pSpeed->LeftValue=speed;
		pSpeed->RightValue=pSpeed->LeftValue*cos(Difference);
	}
	else
	    if (Difference >= pi/2 && Difference < pi)
		{
			pSpeed->LeftValue=-speed;
			pSpeed->RightValue=pSpeed->LeftValue*cos(pi-Difference);
		}
		else
			if (Difference >= pi && Difference < pi*3/2)
			{
				pSpeed->RightValue=-speed;
				pSpeed->LeftValue=pSpeed->RightValue*cos(Difference-pi);
			}
			else
			{
				pSpeed->RightValue=speed;
				pSpeed->LeftValue=pSpeed->RightValue*cos(2*pi-Difference);
			}
	return 1;
}
int CDecisionMakingx::s_MoveOnAngle(dbROBOTPOSTURE *pRobot,double Angle,double speed,dbLRWheelVelocity *pSpeed)
//本函数用于小车沿某一角度方向移动,
//
{
	double Difference;
	Angle = cn_AngleTrim2PI(Angle);
	pRobot->theta = cn_AngleTrim2PI(pRobot->theta);
	Difference = pRobot->theta - Angle;
	if(Difference < -pi) 
		Difference = 2*pi + Difference;
	else if(Difference > pi) 
		Difference = Difference - 2*pi;
	if (fabs(Difference) < m_AngleParameter.AngleError)
	{
		pSpeed->LeftValue = speed;
		pSpeed->RightValue = speed;
	}
	else if(Difference > 0)
	{
		pSpeed->LeftValue = speed;
		pSpeed->RightValue = pSpeed->LeftValue*cos(Difference);
	}
	else if(Difference < 0)
	{
		pSpeed->RightValue = speed;
		pSpeed->LeftValue = pSpeed->RightValue*cos(Difference);
	}
	return 1;
}
//以优余弦曲线到定点
int CDecisionMakingx::Move2Pt(dbROBOTPOSTURE* pROBOTPOSTURE,dbPOINT Target,dbLRWheelVelocity* pLRWheelVelocity)
{
	double theta;
	double dx,dy,dx1,dy1;
	double k1 = 1.0;
	double distance;

	//坐标变换，先平移后转角，以小车中心为原点，小车朝向为y轴
	dx1 = Target.x - pROBOTPOSTURE->x;
	dy1 = Target.y - pROBOTPOSTURE->y;
	dx = dx1*cos(pROBOTPOSTURE->theta-pi/2) + dy1*sin(pROBOTPOSTURE->theta-pi/2);
	dy = -dx1*sin(pROBOTPOSTURE->theta-pi/2) + dy1*cos(pROBOTPOSTURE->theta-pi/2);

	distance = sqrt(dx*dx+dy*dy);
	theta = atan2(dy,dx);	
	if(fabs(fabs(theta) - pi/2) > m_AngleParameter.MaxAngle)
	{
		TurnToPointPD(pROBOTPOSTURE,Target,0,pLRWheelVelocity);
		return 0;
	}
//	if(theta < pi/7) theta *= 2.0;
	if(fabs(dx) < 1.0)
	{