decisionmakingx.cpp

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

	if(angle < angleMax)
	{
		speed_e = kpa*angle + kda*(angle-anglePrev[nRobot]);
		speed = kp*dist+kd*(dist - distPrev[nRobot]); // speed*(angleMax-angle)/angleMax;
	}
	else
	{
		speed_e = kpa*angle + kda*(angle-anglePrev[nRobot]);
		speed = 0;
	}
	distPrev[nRobot] = dist;
	anglePrev[nRobot] = angle;
	
	switch (nQuadrand)
	{
	case 1:
		pSpeed->LeftValue = speed - speed_e/2;
		pSpeed->RightValue = speed + speed_e/2;
		break;
	case 2:
		pSpeed->LeftValue = -speed + speed_e/2;
		pSpeed->RightValue = -speed - speed_e/2;
		break;
	case 3:
		pSpeed->LeftValue = -speed - speed_e/2;
		pSpeed->RightValue = -speed + speed_e/2;
		break;
	case 4:
		pSpeed->LeftValue = speed + speed_e/2;
		pSpeed->RightValue = speed - speed_e/2;
		break;
	}
	
	
	LimitSpeed(pSpeed, MAXSPEED, FALSE);
	
/*	// 缓冲
	double distBuffer = Robot[nRobot].speed / 30;
	if (dist < distBuffer)
	{
		LimitSpeed(pSpeed, 0, 0);
	}
*/
}


/************************************************************************/
/* 到定点函数                                                           */
/* 可调参数：kp4new : 角速度K系数；angle:当角度偏差大于angle时，先转	*/
/************************************************************************/
void CDecisionMakingx::ToPositionN(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  //修改 :李庆赟 2004.6.11
	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;
	
	{
		SendMsg(0, "%5.5f %5.5f %5.5f %5.5f", 
			dist, angle, Robot[nRobot].speedv, Robot[nRobot].speedw);
	}

	if(angle < angleMax)
	{
		speed_e = kpa*angle + kda*(angle-anglePrev[nRobot]);
		speed = speed*(angleMax-angle)/angleMax;
	}
	else
	{
		speed_e = kpa*angle + kda*(angle-anglePrev[nRobot]);
		speed = 0;
	}
	distPrev[nRobot] = dist;
	anglePrev[nRobot] = angle;
	
	switch (nQuadrand)
	{
	case 1:
		pSpeed->LeftValue = speed - speed_e/2;
		pSpeed->RightValue = speed + speed_e/2;
		break;
	case 2:
		pSpeed->LeftValue = -speed + speed_e/2;
		pSpeed->RightValue = -speed - speed_e/2;
		break;
	case 3:
		pSpeed->LeftValue = -speed - speed_e/2;
		pSpeed->RightValue = -speed + speed_e/2;
		break;
	case 4:
		pSpeed->LeftValue = speed + speed_e/2;
		pSpeed->RightValue = speed - speed_e/2;
		break;
	}
	
	
	LimitSpeed(pSpeed, MAXSPEED, TRUE);
	
/*	// 缓冲
	double distBuffer = Robot[nRobot].speed / 30;
	if (dist < distBuffer)
	{
		LimitSpeed(pSpeed, 0, 0);
	}
*/
}

/************************************************************************/
/*  守门员用到定点函数                                                  */
/************************************************************************/
void CDecisionMakingx::ToPositionPDGoal(int nRobot, VecPosition posTarget, 
									   double speed, double endspeed)
{
	VecPosition posRobot(Robot[nRobot].x, Robot[nRobot].y);
	dbLRWheelVelocity* pSpeed = &rbV[nRobot];
	
	// 但球很近的时候，不要调整角度
	if (posRobot.GetDistanceTo(m_posBall) < 20)
		posTarget.SetX(posRobot.GetX());

	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);
	
	if (dist < 1)
		angle = VecPosition::NormalizeAngle(
		PI/2 - Robot[nRobot].theta);
	
	
	double kp4new = 100;

	if (dist > 100) 
		kp4new = 15;
	else if (dist > 50)
		kp4new = 20;
	else if (dist > 30)
		kp4new = 25;
	else if (dist > 20)
		kp4new = 30;
	else 
		kp4new = 100;
	
	if (dist < 5.0 && fabs(angle) < 40)
		kp4new = 5;


/*	double dBufferDist = 50;
	if (dist < dBufferDist)
		speed =  endspeed + dist * (speed - endspeed)/dBufferDist;

	if (speed < endspeed)
		speed = endspeed;
*/	

#ifdef SimuroSot5 
	double angleMax = PI*89./180.;	// 大于此角度先偏转
	double kp = 45, ki=0, kd=250;
	double kpa = 100, kia = 0, kda = 100;
#endif

#ifdef SimuroSot11
	double angleMax = PI*89./180.;	// 大于此角度先偏转
	double kp = 45, ki=0, kd=250;
	double kpa = 100, kia = 0, kda = 100;
#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;


	static double distSum = 0;	
/*	if (posTarget.GetY() > Robot[nRobot].y)
		dist = -dist;
	distSum += dist;
*/	
	if(angle < angleMax)
	{
		speed_e = kpa*angle + kda*(angle-anglePrev[nRobot]);
		speed = kp*dist+ki*distSum+kd*(dist - distPrev[nRobot]); //speed*(1.0 / (1.0 + exp(-3.0 * dist)) - 0.3);
	}
	else
	{
		speed_e = kpa*angle + kda*(angle-anglePrev[nRobot]);
		speed = 0;
	}
	distPrev[nRobot] = dist;
	anglePrev[nRobot] = angle;
	
	SendMsg(0, "%2.2f, %2.2f %d %2.2f %2.2f", dist, angle, nQuadrand, speed, speed_e);
	
	switch (nQuadrand)
	{
	case 1:
		pSpeed->LeftValue = speed - speed_e/2;
		pSpeed->RightValue = speed + speed_e/2;
		break;
	case 2:
		pSpeed->LeftValue = -speed + speed_e/2;
		pSpeed->RightValue = -speed - speed_e/2;
		break;
	case 3:
		pSpeed->LeftValue = -speed - speed_e/2;
		pSpeed->RightValue = -speed + speed_e/2;
		break;
	case 4:
		pSpeed->LeftValue = speed + speed_e/2;
		pSpeed->RightValue = speed - speed_e/2;
		break;
	}

	LimitSpeed(pSpeed, MAXSPEED, FALSE);

	// 缓冲
/*	double distBuffer = Robot[ROBOTNUMBER].speed / 30;
	if (dist < distBuffer)
	{
		LimitSpeed(pSpeed, 0, FALSE);
	}
*/
	
	SendMsg(0, "%2.2f, %2.2f", pSpeed->LeftValue, pSpeed->RightValue);
}

/************************************************************************/
/* 踢球到定点函数                                                       */
/* 可调参数：kp4new, P参数；angle，转动角度								*/
/* angleLimit1,2; distLimit1,2											*/
/************************************************************************/
void CDecisionMakingx::ToPositionNew(int nRobot, VecPosition posTarget, VecPosition posBall, 
									 double speed, int IfEndprocess)
{
	dbLRWheelVelocity* pSpeed = &rbV[nRobot];

//	VecPosition posBall = m_posBall;
	VecPosition posRobot(Robot[nRobot].x, Robot[nRobot].y);

	double distBall2Robot = (posBall - posRobot).GetMagnitude();
	double angleBall2Robot = (posBall - posRobot).GetDirection();
	double angleTarget2Ball = (posTarget - posBall).GetDirection();


	{
		SendMsg(0, "%5.5f %5.5f %5.5f %5.5f", 
			distBall2Robot, angleBall2Robot - Robot[nRobot].theta,
			angleTarget2Ball - angleBall2Robot, Robot[nRobot].speedv);
	}

	double angleE = angleTarget2Ball - angleBall2Robot;
	
	int  sign = Maths::Sign(angleE);
	
/************************************************************************/
/* ToPositionNew函数的目标角度参数函数                                  */
/* 可调参数：angleLimit1,angleLimit2, distLimit1, distLimit2			*/
/************************************************************************/
	double anglelimit1 = PI*0.5;
	double anglelimit2 = PI*.12;
	double distlimit1 = 200;
	double distlimit2 = 10;
	
	double ktheta =(distlimit1 - distBall2Robot)/(distlimit1 - distlimit2);
	ktheta = Maths::Limit(ktheta, 0, 1);
	
	double tempp = 0;
	if(distBall2Robot <= distlimit1)
	{
		tempp = (fabs(angleE) - anglelimit2)/(anglelimit1 - anglelimit2);
		tempp = Maths::Limit(tempp, 0, 1);
		tempp *= PI/2;

		tempp = ktheta*tempp;
	}
/*	double l1,l3;
	l1 = 20;	
	l3=1-(l1-distBall2Robot-fabs(angleE)*150)/l1;
	l3 = Maths::Limit(l3, 0, 1);
	tempp *= l3;
*/
	///< 当distRobot2Ball > distLimit2 || angleE < angleLimit2时，笔直冲向球
	///< 当distRobot2Ball < distLimit1 && angleE > angleLimit1时，最多转角PI/2

	//计算下个周期的目标角度
	double disiredAngle = angleBall2Robot - sign*tempp;

	//计算角度偏差
	double Angle_e;
	Angle_e = disiredAngle - Robot[nRobot].theta;
	Angle_e = VecPosition::NormalizeAngle(Angle_e);


	//计算左右轮速度差并计算出左右轮速度
	double angle = PI*.5;
//	if(Robot[nRobot].x<7||Robot[nRobot].x>PITCH_LENGTH-7||Robot[nRobot].y<7||Robot[nRobot].y>PITCH_WIDTH-7)
//		angle = PI*.27;

	double speed_e;

	double kp4new = 80;

	double distLimit3 = 35;
	double distLimit4 = 15;
	double kp4Max = 90;
	double kp4Max2 = 30;

	if (Angle_e > -PI && Angle_e < -PI/2)
	{
		if (distBall2Robot < distLimit3 && distBall2Robot > distLimit4)
			kp4new = kp4new * (Angle_e + PI)/(PI/2) * (distLimit3/distBall2Robot);
		else if (distBall2Robot < distLimit4)
			kp4new = kp4Max2;

		kp4new = Maths::Limit(kp4new, 0, kp4Max);

		speed_e = kp4new*(Angle_e + PI);
		if(fabs(Angle_e + PI)>angle)
			speed = 0;
		else
			speed = speed*(PI/2-fabs(Angle_e + PI))/angle;
		
		pSpeed->LeftValue = speed + speed_e/2;
		pSpeed->RightValue = speed - speed_e/2;
		pSpeed->LeftValue =- pSpeed->LeftValue;
		pSpeed->RightValue =- pSpeed->RightValue;
	}
	else if (Angle_e > -PI/2 && Angle_e < 0)
	{
		if (distBall2Robot < distLimit3 && distBall2Robot > distLimit4)
			kp4new = kp4new * (Angle_e + PI)/(PI/2) * (distLimit3/distBall2Robot);
		else if (distBall2Robot < distLimit4)
			kp4new = kp4Max2;

		kp4new = Maths::Limit(kp4new, 0, kp4Max);
		
		speed_e = kp4new*(-Angle_e);
		if(fabs(-Angle_e)>angle)
			speed = 0;
		else
			speed = speed*(PI/2-fabs(-Angle_e))/angle;
		
		pSpeed->LeftValue = speed + speed_e/2;
		pSpeed->RightValue = speed - speed_e/2;
	}
	else if (Angle_e >= 0 && Angle_e < PI/2)//角度偏差在第一象限
	{
		if (distBall2Robot < distLimit3 && distBall2Robot > distLimit4)
			kp4new = kp4new * (Angle_e + PI)/(PI/2) * (distLimit3/distBall2Robot);
		else if (distBall2Robot < distLimit4)
			kp4new = kp4Max2;

		kp4new = Maths::Limit(kp4new, 0, kp4Max);

		speed_e = kp4new*Angle_e;
		if(fabs(Angle_e)>angle)
			speed = 0;
		else
			speed = speed*(PI/2-fabs(Angle_e))/angle;

		pSpeed->LeftValue = speed - speed_e/2;
		pSpeed->RightValue = speed + speed_e/2;
	}
	else if(Angle_e >= PI/2 && Angle_e <= PI)
	{
		if (distBall2Robot < distLimit3 && distBall2Robot > distLimit4)
			kp4new = kp4new * (Angle_e + PI)/(PI/2) * (distLimit3/distBall2Robot);
		else if (distBall2Robot < distLimit4)
			kp4new = kp4Max2;

		kp4new = Maths::Limit(kp4new, 0, kp4Max);

		speed_e = kp4new*(PI - Angle_e);
		if(fabs(PI - Angle_e)>angle)
			speed = 0;
		else
			speed = speed*(PI/2-fabs(PI-Angle_e))/angle;

		pSpeed->LeftValue = speed - speed_e/2;
		pSpeed->RightValue = speed + speed_e/2;
		pSpeed->LeftValue =- pSpeed->LeftValue;
		pSpeed->RightValue =- pSpeed->RightValue;
	}
	
//	SendMsg(m_nTimer, "angleE=%2.2f, tempp = %2.2f, kp4 = %2.2f, speede=%2.2f", 
//		angleE, tempp, kp4new, speed_e);


	EndProcess(IfEndprocess,nRobot,posTarget, posBall);	

	LimitSpeed(pSpeed, MAXSPEED);
}



/************************************************************************/
/* 末端处理(不能应用到球在车后的情况)                                   */
/************************************************************************/
void CDecisionMakingx::EndProcess(int IfEndprocess,int nRobot,
								  VecPosition posTarget, VecPosition posBall)