decisionmakingx.cpp

仿真机器人的经典代码

	}
	if(pSpeed->RightValue>vemax)
	{
		pSpeed->RightValue = vemax;
		pSpeed->LeftValue = pSpeed->RightValue - fabs(speed_e);
	}
	//修改成自然坐标系
	ball = ballPt;
	robot->y = 180 - robot->y;
	robot->theta = 2*pi - robot->theta;
	ball.y = 180 - ball.y;
	directionpt.y = 180 - directionpt.y;
	robot->theta = cn_AngleTrim2PI(robot->theta);
	if(IfEndprocess)
		EndProcess(robot,directionpt,ball,pSpeed);	
	return 1;
}
//
int CDecisionMakingx::ToPositionN(dbROBOTPOSTURE* robot,dbPOINT directionpt, double samespeed, dbLRWheelVelocity* pSpeed)
{
	
	int vemax;
	vemax = 124;
	double dist;
	double Dx,Dy;
	double Anglerb2target;
	robot->y = 180 - robot->y;
	robot->theta = 2*pi - robot->theta;
	directionpt.y = 180 - directionpt.y;
	robot->theta = cn_AngleTrim2PI(robot->theta);
	//计算机器人到目标点的角度
	Dx = directionpt.x - robot->x;Dy = directionpt.y - robot->y;
	dist = sqrt(Dx*Dx+Dy*Dy);


	double kp4new;

	
	kp4new = 100.0;
	if(samespeed<=40)
	kp4new = 100;// kp4new*samespeed/vemax;


	//计算targetpt到directionpt的角度
	Anglerb2target = atan2(Dy ,Dx);
	Anglerb2target = cn_AngleTrim2PI(Anglerb2target);

	//计算下个周期的目标角度
	double disiredAngle;
	disiredAngle = Anglerb2target;
	disiredAngle = cn_AngleTrim2PI(disiredAngle);
	//计算角度偏差
	double Angle_e;
	Angle_e = disiredAngle - robot->theta;
	Angle_e = cn_AngleTrim2PI(Angle_e);
	//计算左右轮速度差并计算出左右轮速度
	
	double ka,limitation,a;
	a = pi*0.3;
	limitation = 100;
	ka=samespeed;
	double speed_e;
	if(Angle_e <= pi/2)//角度偏差在第一象限
	{
		speed_e = kp4new*Angle_e;
		speed_e = Limit(speed_e,limitation);
		if(a-fabs(Angle_e)>0)
			samespeed = ka*(a-fabs(Angle_e))/a;
		else samespeed = 0;
		
		pSpeed->LeftValue = samespeed + speed_e/2;
		pSpeed->RightValue = samespeed - speed_e/2;
	}
	else if(Angle_e <= pi)
	{
		speed_e = kp4new*(pi - Angle_e);
		speed_e = Limit(speed_e,limitation);
		if(a-fabs(pi-Angle_e)>0)
			samespeed = ka*(a-fabs(pi-Angle_e))/a;
		else samespeed = 0;

		pSpeed->LeftValue = samespeed + speed_e/2;
		pSpeed->RightValue = samespeed - speed_e/2;
		pSpeed->LeftValue =- pSpeed->LeftValue;
		pSpeed->RightValue =- pSpeed->RightValue;
	}
	else if(Angle_e<3*pi/2)
	{
		speed_e = kp4new*(Angle_e - pi);
		speed_e = Limit(speed_e,limitation);
		if(a-fabs(Angle_e - pi)>0)
			samespeed = ka*(a-fabs(Angle_e - pi))/a;
		else samespeed = 0;

		pSpeed->LeftValue = samespeed - speed_e/2;
		pSpeed->RightValue = samespeed + speed_e/2;
		pSpeed->LeftValue =- pSpeed->LeftValue;
		pSpeed->RightValue =- pSpeed->RightValue;
	}
	else
	{
		speed_e = kp4new*(2*pi - Angle_e);
		speed_e = Limit(speed_e,limitation);
		if(a-fabs(2*pi - Angle_e)>0)
			samespeed = ka*(a-fabs(2*pi - Angle_e))/a;
		else samespeed = 0;

		pSpeed->LeftValue = samespeed - speed_e/2;
		pSpeed->RightValue = samespeed + speed_e/2;
	}
	
	if(pSpeed->LeftValue>vemax)
	{
		pSpeed->LeftValue = vemax;
		pSpeed->RightValue = pSpeed->LeftValue - fabs(speed_e);
	}
	if(pSpeed->LeftValue<-vemax)
	{
		pSpeed->LeftValue = -vemax;
		pSpeed->RightValue = pSpeed->LeftValue + fabs(speed_e);
	}
	if(pSpeed->RightValue<-vemax)
	{
		pSpeed->RightValue = -vemax;
		pSpeed->LeftValue = pSpeed->RightValue + fabs(speed_e);
	}
	if(pSpeed->RightValue>vemax)
	{
		pSpeed->RightValue = vemax;
		pSpeed->LeftValue = pSpeed->RightValue - fabs(speed_e);
	}
	
	return 1;
}
//守门员用
int CDecisionMakingx::ToPositionPDGoal(dbROBOTPOSTURE* pROBOTPOSTURE,dbPOINT Target,double startspeed,double endspeed,dbLRWheelVelocity* pLRWheelVelocity)
{
	int clock_sign,move_sign;
	double theta,theta_e1;//e1为当前角度误差
	static double theta_e2 = 0;//e2为上一周期角度误差
	double dx,dy,dx1,dy1,distance;
	double same_speed;


	//坐标变换，以小车中心为原点，小车朝向为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);
	if(distance > m_MoveParameter.max_distanceG)
		same_speed = startspeed;
	else
		same_speed =  distance/m_MoveParameter.max_distanceG*startspeed;
	if(same_speed < endspeed) same_speed = endspeed;
	theta = atan2(dy,dx);	
	if(fabs(fabs(theta) - pi/2) > m_AngleParameter.MaxAngle)
	{
		TurnToPointPD(pROBOTPOSTURE,Target,NOCLOCK,pLRWheelVelocity);
		pLRWheelVelocity->LeftValue /= 3;
		pLRWheelVelocity->RightValue /= 3;
		return 0;
	}
	theta = cn_AngleTrim2PI(theta);
	if(theta <= pi/2)//第一象限
	{
		move_sign = 1;
		clock_sign = 1;
		theta_e1 = pi/2 - theta;
	}
	else if(theta <= pi)//第二象限
	{
		move_sign = 1;
		clock_sign = -1;
		theta_e1 = theta - pi/2;
	}
	else if(theta <= 3*pi/2)//第三象限
	{
		move_sign = -1;
		clock_sign = 1;
		theta_e1 = 3*pi/2 - theta;
	}
	else//第四象限
	{
		move_sign = -1;
		clock_sign = -1;
		theta_e1 = theta - 3*pi/2;
	}
//	if(theta_e1 > 20.0/180.0*pi)
		same_speed = same_speed*(1-theta_e1/(pi/2));
	double kp,kd;
	kp = m_MoveParameter.kp4pospdG;
	kd = m_MoveParameter.kd4pospdG;
	if(distance < 20 && fabs(theta_e1) < pi/6)
	{
		kp *= 0.3;
		kd *= 0.3;
	}
	pLRWheelVelocity->LeftValue = same_speed*move_sign + clock_sign*(kp*theta_e1 + kd*(theta_e1- theta_e2));
	pLRWheelVelocity->RightValue = same_speed*move_sign - clock_sign*(kp*theta_e1 + kd*(theta_e1- theta_e2));
	
	//保存本周期角度误差，下一周期作微分用	
	theta_e2 = theta_e1;
	return 0;
}
//基本动作函数结束
//////////////////////////////////////////////////////////////
//                 技术动作函数                             //
//////////////////////////////////////////////////////////////
/************************************************************************************
函数名	：BoundProcess(int which, ballInformation &ball)
创建日期：25/3/2006
作者	：邓星桥
功能	：边界处理
参数	：	which		机器人号码
			ball		球
**************************************************************************/
int CDecisionMakingx::BoundProcess(dbROBOTPOSTURE *pRobot, BallInformation Ball,int field,dbLRWheelVelocity *pSpeed)
//本函数用于处理边界球，成功返回1
//pRobot为小车的位置信息，pBall为球的位置信息，field为球所在的边界区号，pSpeed为返回的左右轮速
//     |--------------|-------------------------------------------|-------------|
//     |LUpCorner=5   |                  UpBound=1                |RUpCorner=3  |
// |---|--------------|--------------|---------------|------------|-------------|---|
// |   |LZone=10      |BackField=7   |MidField=8     |FrontField=9| RZone=11    |   |
// |   |              |              |               |            |             |   |
// |---|--------------|--------------|---------------|------------|-------------|---|
//     |LDownCorner=6 |                  DownBound=2              |RDownCorner=4|
//     |--------------|-------------------------------------------|-------------|
{
/*	dbPOINT point,point1,point2;
	point.x=Ball.x;
	point.y=Ball.y;
	point2.x=pRobot->x;
	point2.y=pRobot->y;
	switch (field)
	{
	case LUpCorner:
		{
			dbPOINT temp;
			temp.x = Ball.x;temp.y = Ball.y - 10;
			pRobot->theta = cn_AngleTrim2PI(pRobot->theta);
			if(cn_2PointsDist(point2,temp) < 4.0)				
			{
				if(pRobot->theta > pi) 
					pRobot->theta = 2*pi - pRobot->theta;
				if(fabs(pRobot->theta - pi/2) < 20.0/180.0*pi && cn_2PointsDist(point2,point) > 4.0)
					Move2Pt(pRobot,point,pSpeed);
				else if(fabs(pRobot->theta - pi/2) > 20.0/180.0*pi)
					TurnToAnglePD(pRobot,pi/2,NOCLOCK,pSpeed);
				else if(cn_2PointsDist(point2,point) < 4.0)
					Turn(m_AngleParameter.MaxAngleSpeed,CLOCKWISE,pSpeed);
			}
			else
				ToPositionPD(pRobot,temp,5.0,pSpeed);
			break;
		}
	case LDownCorner:
		{	
			dbPOINT temp;
			temp.x = Ball.x;temp.y = Ball.y + 10;
			pRobot->theta = cn_AngleTrim2PI(pRobot->theta);
			if(cn_2PointsDist(point2,temp) < 4.0)				
			{	
				if(pRobot->theta > pi) 
					pRobot->theta = 2*pi - pRobot->theta;
				if(fabs(pRobot->theta - pi/2) < 20.0/180.0*pi && cn_2PointsDist(point2,point) > 4.0)
					Move2Pt(pRobot,point,pSpeed);
				else if(fabs(pRobot->theta - pi/2) > 20.0/180.0*pi)
					TurnToAnglePD(pRobot,pi/2,NOCLOCK,pSpeed);
				else if(cn_2PointsDist(point2,point) < 4.0)
					Turn(m_AngleParameter.MaxAngleSpeed,ANTICLOCK,pSpeed);
			}
			else
				ToPositionPD(pRobot,temp,5.0,pSpeed);
			break;
		}
	case UpBound:
		{		
			point1.y = Ball.y ;
			point1.x = Ball.x - BoundMinDist+1;
			if(pRobot->x < Ball.x - 15.0 && pRobot->y < Ball.y - 15.0)
				ToPositionPD(pRobot,point1,20.0,pSpeed);
			else
				ToPositionPD(pRobot,Ball,5.0,pSpeed);
		break;
		}
	case DownBound:
		{
			point1.y = Ball.y ;
			point1.x = Ball.x - BoundMinDist+1;
			if(pRobot->x < Ball.x - 15.0 && pRobot->y < Ball.y + 15.0)
				ToPositionPD(pRobot,point1,10.0,pSpeed);
			else
				ToPositionPD(pRobot,Ball,5.0,pSpeed);
		break;
		}
	case RUpCorner:
		{
			m_decisionparamter.MRtarget.x = 100;
			m_decisionparamter.MRtarget.y = 110;
			pRobot->theta = cn_AngleTrim2PI(pRobot->theta);
			if(cn_2PointsDist(point2, m_decisionparamter.MRtarget) < 3.0)
				if(pRobot->theta < pi/2 || pRobot->theta > 3*pi/2)
					TurnToAnglePD(pRobot,0.0,NOCLOCK,pSpeed);
				else
					TurnToAnglePD(pRobot,pi,NOCLOCK,pSpeed);
			else
				ToPositionPD(pRobot,m_decisionparamter.MRtarget,5.0,pSpeed);

			break;
		}
	case RDownCorner:
		{
			m_decisionparamter.MRtarget.x = 100;
			m_decisionparamter.MRtarget.y = 20;
			pRobot->theta = cn_AngleTrim2PI(pRobot->theta);
			if(cn_2PointsDist(point2, m_decisionparamter.MRtarget) < 3.0)
				if(pRobot->theta < pi/2 || pRobot->theta > 3*pi/2)
					TurnToAnglePD(pRobot,0.0,NOCLOCK,pSpeed);
				else
					TurnToAnglePD(pRobot,pi,NOCLOCK,pSpeed);	
			else
				ToPositionPD(pRobot,m_decisionparamter.MRtarget,5.0,pSpeed);

			break;
		}
	default:
		return 0;
	}*/
	return 1;
}
/************************************************************************************
函数名	：GoalieAction(robotInformation &robot, ballInformation &ball, dbPOINT &proball)
创建日期：19/4/2006
作者	：邓星桥
功能	：守门
参数	：	robot		机器人位姿
			ball		球
**************************************************************************/
int CDecisionMakingx::GoalieAction1(dbROBOTPOSTURE* pRobotInford, BallInformation ball,dbLRWheelVelocity *pSpeed)
{	
	//int homeline =8;	G_OFFSET
	int		Error_y = 2;						//block goal area up and down corner
	int		xError = 3;						// homePt.x+xError and homePt.x-xError;
	int		gtError = 2, glError = 2;			// gate corner and goal corner +-
	dbPOINT	target;
	dbPOINT	homePt;
	double	dx,dy;
	static double  turn_E2 = 0;
	int		goToken=0;			// go first then turn;

	homePt.x = G_OFFSET;
	homePt.y = CENTER_Y;

	target = homePt;
	///////////////////////////////////

	pRobotInford->theta = cn_AngleTrim2PI(pRobotInford->theta);
	if(ball.x > CENTER_X + 40)
	{
		dx = pRobotInford->x - homePt.x;
		dy = pRobotInford->y - homePt.y;
		if(dx*dx + dy*dy < 2*2)
			if(pRobotInford->theta < pi)
				TurnToAnglePD(pRobotInford,pi/2,NOCLOCK,pSpeed);
			else
				TurnToAnglePD(pRobotInford,3*pi/2,NOCLOCK,pSpeed);
		else
			ToPositionPD(pRobotInford,homePt,m_MoveParameter.V_max,0.0,pSpeed);	
	}
	else	// ball in process area;
	{	// Forcast ball next postition
		if(m_decisionparamter.oldBallPt[0].x > m_decisionparamter.oldBallPt[3].x && m_decisionparamter.oldBallPt[3].x > m_decisionparamter.oldBallPt[6].x)				 // GG new add
		{
			target.y = GetCrossPtWithGLLine(G_OFFSET);			 //update oldball previous
		}
		else
			target.y = 0;		
		if(target.y > GATE_UP_LINE || target.y < GATE_DN_LINE)//交点出了门区
		{
			target.y = ball.y;
			if(ball.x < homePt.x + 5)//x方向离门很近，目标点在禁区里2厘米处
			{
				if(target.y < GOAL_DN_LINE) 
				{
					target.y = GOAL_DN_LINE + glError;
					if(pRobotInford->y < GATE_DN_LINE - glError ) //跑出门区
						goToken = 2;//回目标点
				}
				else if(target.y > GOAL_UP_LINE) 
				{
					target.y = GOAL_UP_LINE - glError;
					if(pRobotInford->y > GATE_UP_LINE + glError ) 
						goToken = 2;						
				}
			}
			else if(ball.x > homePt.x + 20)//离门>25厘米
			{
				if(target.y < GATE_DN_LINE) 
					target.y = GATE_DN_LINE + 5;		  // GGG
				else if(target.y > GATE_UP_LINE) 
					target.y = GATE_UP_LINE - 5; // GGG
				goToken = 1;
			}
			else
			{
				if(target.y < GATE_DN_LINE) 
					target.y = GATE_DN_LINE - gtError;
				else if(target.y > GATE_UP_LINE) 
					target.y = GATE_UP_LINE + gtError;
			}
		}		
	///////////////////////////////////
	if(target.y > GATE_UP_LINE + 3)target.y = GATE_UP_LINE + 3;
	else if(target.y < GATE_DN_LINE - 3)target.y = GATE_DN_LINE - 3;
	double	e1;
	static double e2 = 0;
	static double   turn_E = 0;
	double	ex;
	double	k = 0;
	double	Kd = 1.0;	//.08;