📄 decisionmakingx.cpp

📁 仿真机器人的经典代码
💻 CPP
📖 第 1 页 / 共 5 页
字号:
上一页 1 2 3 45
		if(k==0)
			s_Move2Pt(pRobot,ball,m_MoveParameter.V_max,pSpeed);

	return 0;*/
	
		dbPOINT	goal,ballPt;
		double delta,angletemp;
		delta = 10;
		ballPt = ball;
		goal.x = wallright + 3;
		goal.y = walltop/2;
		angletemp = Getpt2ptAngle(ballPt,goal);
		//ballPt.x = ballPt.x - delta*sin(angletemp);
		//ballPt.y = ballPt.y - delta*cos(angletemp);
		double deltax;
		deltax = 10;
		if((ball.x>wallright - deltax)&&(ball.y>CENTER_Y - goal_y_widthM/2-deltax&&ball.y<CENTER_Y + goal_y_widthM/2 + deltax))
		{
			if(ball.y<=CENTER_Y)
				goal.y = ball.y + deltax;
			else
				goal.y = ball.y - deltax;
			goal.x = wallright + 8;
			ToPositionNew(pRobot,ballPt,goal,120,1,pSpeed);
			return 1;
		}
		ToPositionNew(pRobot,ballPt,goal,120,2,pSpeed);	
		/*if(pRobotInford.x>ball.x)
			Vect_MidShoot2(pRobotInford,ball,goal,0,pSpeed);*/
		return 1;

}
///////////////////////////////////////////////////////
//函数   DribleBall()
//函数名 带球至对方球门
///////////////////////////////////////////////////////
int CDecisionMakingx::DribleBall(dbROBOTPOSTURE *pRobot,ballInformation ball ,dbLRWheelVelocity *pSpeed)
{

	return 1;
}
///////////////////////////////////////////////////////
//函数   Defence()
//函数名 防守
///////////////////////////////////////////////////////
int CDecisionMakingx::Defence(dbROBOTPOSTURE *pRobot,ballInformation ball ,dbLRWheelVelocity *pSpeed)
{
		double		theta_rb;		
		double		dx,dy;
		double		dist;
		int			IsClockWise = 0;
		dbPOINT		target;
		double		moveTheta,theta_e;
		double		k = 1.0; 
///////////////////////////////////////////
		dx = ball.x - pRobot->x;
		dy = ball.y - pRobot->y;
		dist = sqrt(dx*dx + dy*dy);

		target.x = ball.x;
		target.y = ball.y;

		theta_rb = atan2(dy,dx);	 
		theta_rb = cn_AngleTrim2PI(theta_rb);
		pRobot->theta = cn_AngleTrim2PI(pRobot->theta);
///////////////////////////////////////////
	if(ball.x > pRobot->x + 5)
	{
		if((theta_rb < 65.0/180*pi && theta_rb >= 0 ) || (theta_rb < 2*pi && theta_rb > 300.0/180*pi)) // 55-- 305 
		{
			theta_e = theta_rb - pRobot->theta;
			theta_e = cn_AngleTrim2PI(theta_e);
			if(theta_e > pi) theta_e -= pi;
			if(theta_e > pi/2)	theta_e = pi - theta_e;

			if(theta_e < pi/6)						// Deb %%% modify 30
			{
				Move2Pt(pRobot,target,m_MoveParameter.V_max,pSpeed);
				return  0;
			}
		}

		double tempC = 0.5*dist/cos(theta_rb);
		if( tempC < 0 ) tempC =0;	// should +		
		target.x = ball.x - tempC;
		if(target.y < wallbottom + BOUND1) 
			target.y= wallbottom + BOUND1;
		else if(target.y > walltop - BOUND1)
			target.y= walltop - BOUND1;
		else if(target.y < GOAL_UP_LINE + 2 && target.y > GOAL_DN_LINE - 2)
		{
			if(target.x < goal_x_widthM + 4) 
				target.x = goal_x_widthM + 4;//2
			if(pRobot->x < goal_x_widthM + 5) 
			{  
				theta_e = pRobot->theta;			
				if(theta_e > pi) theta_e -= pi;
				theta_e -= pi/2;
				if(fabs(theta_e) > pi/6)
				{ 
					TurnToAnglePD(pRobot,pi/2,NOCLOCK,pSpeed);
					return 0;
				}
				else			
					target.x = ball.x;	// ????	
			}
		}
		else 
		{	
			if(target.x < wallleft + BOUND1) 
				target.x = wallleft + BOUND1;
		}		
		ToPositionPD(pRobot,target,m_MoveParameter.V_max,10.0,pSpeed);	// Vbase pt
		return 0;
	}
	else
	{
		if(pRobot->x < goal_x_widthM + 4) 
		    if( pRobot->y < GOAL_UP_LINE + 2 && pRobot->y > GOAL_DN_LINE - 2)
			{
				 theta_e = pRobot->theta;
				if(theta_e > pi) theta_e -= pi;
					theta_e -= pi/2;
				if(fabs(theta_e) > pi/6)			
					TurnToAnglePD(pRobot,pi/2,NOCLOCK,pSpeed);
				else
				{
					target.x = goal_x_widthM;//+2
					if(ball.y > GOAL_UP_LINE + 2)
						target.y = GOAL_UP_LINE + 8;
					else if(ball.y < GOAL_DN_LINE - 2)
						target.y = GOAL_DN_LINE - 8;
					else
						target.y = ball.y;
//					ToPositionPD(pRobot,target,m_MoveParameter.V_max,10.0,pSpeed);	// Vbase pt
					Move2Pt(pRobot,target,pSpeed);
				}
				return 0;
			}
			else if( ball.x < goal_x_widthM)
			{
				target.y = 0;
				if(ball.y > GOAL_UP_LINE + 6)
					target.y = GOAL_UP_LINE + 8;
				else if(ball.y < GOAL_DN_LINE - 6)
					target.y = GOAL_DN_LINE - 8;
				//else						//	????
				//	target.y= ball.y;			
				if(target.y != 0)
				{
					ToPositionPD(pRobot,target,m_MoveParameter.V_max,10.0,pSpeed);	// Vbase pt
					return 0;
				}
			} 

		if(dist > 40) k = 0;
		else k = 15;//12
		// according to the position of ball compared with robot;
		if(ball.y > UP_LINE)
			IsClockWise = 1;		
		else if(ball.y < DN_LINE)		
			IsClockWise = -1;	
		else
		{
			if(pRobot->y < ball.y ) 
				IsClockWise = 1;
			else
				IsClockWise = -1;
		}
		moveTheta = theta_rb + k*IsClockWise/dist*(pi/2);
		if(dist > 50)
			MoveOnAngle(pRobot,moveTheta,m_MoveParameter.V_max,pSpeed);
		else
			MoveOnAngle(pRobot,moveTheta,m_MoveParameter.V_max/2,pSpeed);
	}
	return 0;
}
//末端处理(不能应用到球在车后的情况)
int CDecisionMakingx::EndProcess(dbROBOTPOSTURE *pRobot,dbPOINT shoot_target,dbPOINT ballPt,dbLRWheelVelocity *pSpeed)
{
		double	dist,distE,anglerb2ball,anglerb2target,angle1,angle2,angle3,maxe,maxd,maxspeed;
		dbPOINT rbPt,EGoal;
		maxspeed = 124;
		EGoal.x = wallright + 3;
		EGoal.y = walltop/2;
		maxd = 15;
		maxe = 3.7;
		dist = distRobot2Pt(*pRobot,ballPt);//求出球和小车的距离
		rbPt.x = pRobot->x;
		rbPt.y = pRobot->y;
		anglerb2ball = Getpt2ptAngle(rbPt,ball);//小车指向球的方向
		anglerb2target = Getpt2ptAngle(rbPt,EGoal);//小车指向目标的方向

		angle1 = cn_AngleTrim2PI(anglerb2ball - anglerb2target);

		if(angle1>pi&&angle1<=2*pi)
			angle1 =angle1 - 2*pi;

		angle2 = cn_AngleTrim2PI(pRobot->theta);
		angle3 = cn_AngleTrim2PI(angle2 - anglerb2ball);
		distE = fabs(dist*sin(angle3));
		//球和车距离很近，车在对方门区附近，车在球后，车和球的连线与右边界的交点在对方球门附近
		if(dist<=maxd&&pRobot->x>CENTER_X+wallright/4&&pRobot->x<ballPt.x&&fabs(angle1)<pi*0.45)
			TurnTo(pRobot,ball,EGoal,124,pSpeed);
		//球和车距离很近，球在车的轨迹范围内,射门角度不是很大
		if(dist<=maxd&&distE<=maxe&&fabs(angle1)<pi*0.45)
		{	//正面向球，
			if(angle3<pi*0.2&&angle3>=0||angle3>pi*1.8&&angle3<2*pi)
			{
				if(angle1>0)
				{
					pSpeed->LeftValue = maxspeed;
					pSpeed->RightValue = pSpeed->LeftValue*fabs(cos(angle1));
				}
				else 
				{
					pSpeed->RightValue = maxspeed;
					pSpeed->LeftValue = pSpeed->RightValue*fabs(cos(angle1));
				}
			}
			//后面向球
			else if(angle3>pi - pi*0.2&&angle3<pi+pi*0.2)
			{
				if(angle1>0)
				{
					pSpeed->RightValue = -maxspeed;
					pSpeed->LeftValue = pSpeed->RightValue*fabs(cos(angle1));
				}
				else
				{
					pSpeed->LeftValue = -maxspeed;
					pSpeed->RightValue = pSpeed->LeftValue*fabs(cos(angle1));
		
				}
			}
		}

		//角度特别好的处理
		//正面向球
		if((angle3<pi*0.2&&angle3>=0||angle3>pi*1.8&&angle3<2*pi)&&fabs(angle1)<pi*0.45&&distE<=maxe&&(angle2>=0&&angle2<=pi*0.45||angle2>=pi*1.55&&angle2<=2*pi))
		{
			double temp;
			temp = pRobot->y+(wallright-pRobot->x)/tan(angle2);
			if(temp>=GATE_DN_LINE+1&&temp<=GATE_UP_LINE-1)
				pSpeed->LeftValue = pSpeed->RightValue = maxspeed;
		}
		//后面向球
		else if((angle3>pi - pi*0.2&&angle3<pi+pi*0.2)&&fabs(angle1)<pi*0.45&&distE<=maxe&&angle2>=pi*0.55&&angle2<=pi*1.45)
		{
			double temp;
			temp = pRobot->y+(wallright-pRobot->x)/tan(angle2);
			if(temp>=GATE_DN_LINE+1&&temp<=GATE_UP_LINE-1)
			pSpeed->LeftValue = pSpeed->RightValue = -maxspeed;
		}
	return 1;
}
//技术动作函数结束
//////////////////////////////////////////////////////////////
//                     运算函数                             //
/////////////////////////////////////////////////////////////
//以坐标变换后的目标点为参数求经过原点（车的中心点）和目标点并与y轴相切的圆的半径，半径为正
double  CDecisionMakingx::SolveRadius(dbPOINT Target)
{
	double radius;
	dbPOINT temp;
	Formulation formu;
	temp.x=0;temp.y=0;//原点
	if(fabs(Target.x) <= 0.0000001)//目标点在坐标变换后的y轴上，为无穷大
	{
		radius=-1;
		return radius;
	}
	else if(fabs(Target.y) <= 0.0000001)//目标点在坐标变换后的x轴上
	{
		radius = fabs(Target.x)/2;
		return radius;
	}
	else 
	{
		//求在新坐标系下车和目标点的线段的中垂线方程
		MiddleVerticalLine(temp, Target, &formu);
		//求中垂线在新x轴的截距并得到半径
		radius=fabs(formu.c/formu.a);						
	}
	return(radius);
}
/************************************************************************************
函数名	：GetCrossPtWithGLLine()
创建日期：30/4/2006
作者	：邓星桥
功能	：预测球的运动曲线与我方球门的交点
**************************************************************************/
double CDecisionMakingx::GetCrossPtWithGLLine()
{
	double detaX,target_y;
	if(ball.x < 30 /*&& (ball.y < DN_LINE || ball.y > UP_LINE)*/)
	{
		target_y = ball.y;
		return target_y;
	}
	if(fabs(ballCharacter.Formu.a/ballCharacter.Formu.b) > tan(75.0/180.0*pi))
	{
		detaX = 8;
//		target_y = ball.y;
//		return target_y;
	}
    else if(fabs(ballCharacter.Formu.a/ballCharacter.Formu.b) > tan(50.0/180.0*pi))
		detaX = 6;
	else if(fabs(ballCharacter.Formu.a/ballCharacter.Formu.b) > tan(35.0/180.0*pi))
		detaX = 4;
	else if(fabs(ballCharacter.Formu.a/ballCharacter.Formu.b) > tan(20.0/180.0*pi))
		detaX = 2;
	else
		detaX = 0;
	target_y = -(ballCharacter.Formu.a * (G_OFFSET + detaX) + ballCharacter.Formu.c)/ballCharacter.Formu.b;
	return target_y; 	
}

double CDecisionMakingx::GetCrossPtWithGLLine(double x)
{	 	
	double target_y = -(ballCharacter.Formu.a * (x+6) + ballCharacter.Formu.c)/ballCharacter.Formu.b;
	return target_y;
}
/************************************************************************************
函数名	：GetMidPoint(robotInformation &robot, ballInformation &ball, int deta_y)
创建日期：30/4/2006
作者	：邓星桥
功能	：得到中分点
参数	：	robot		机器人位姿
			ball		球位姿 	
**************************************************************************/
dbPOINT CDecisionMakingx::GetMidPoint(dbROBOTPOSTURE* pRobotInford,BallInformation ball, double deta_y)
{
	double		dx,dy;
	double	    dist;
	double		theta_br;	
	double		theta_gb;
	double		theta_e;
	double  	theta1,theta2;
	dbPOINT     target;
///////////////////////////////////////////
	dy = pRobotInford->y - ball.y;
	dx = pRobotInford->x - ball.x;
	//求球与机器人的距离
	dist = sqrt(dx*dx + dy*dy);
	target.x = ball.x; target.y = ball.y;
	//求球到机器人连线的方向角               
	theta_br = atan2(dy,dx);	 
	theta_br = cn_AngleTrim2PI(theta_br);

	dy = ball.y - RGATE_Y - deta_y;
	dx = ball.x - RGATE_X;
	//求球到对方大门中点的角度
	theta_gb = atan2(dy,dx);	 
	theta_gb = cn_AngleTrim2PI(theta_gb);

	theta1 = theta_gb - 80.0*pi/180;//90;
	theta2 = theta_gb + 80.0*pi/180;//90;

	if(theta_br > theta1 && theta_br < theta2)
	{
		theta_e = theta_br - theta_gb;
		double tempC = fabs(0.5*dist/cos(theta_e));
		
		target.x = ball.x + tempC * cos(theta_gb);
		target.y = ball.y + tempC * sin(theta_gb);

		if(target.x < wallleft + BOUND1) 
			target.x = wallleft + BOUND1;
		if(target.y < wallbottom + BOUND1) 
			target.y = wallbottom + BOUND1;
		else if(target.y > walltop - BOUND1)
			target.y = walltop - BOUND1;

		dy = ball.y - target.y;
		dx = ball.x - target.x;

		if( dx*dx + dy*dy < 4*4)	// may need to modigy ###
		{
			target.x = RGATE_X;			
			target.y = RGATE_Y;
		}	
	}
	else
		target.x =-1;

	return target;
}
/************************************************************************************
函数名	：GetMidPoint(robotInformation &robot, ballInformation &ball, int deta_y)
创建日期：8/5/2006
作者	：邓星桥
功能	：得到中分点
参数	：	robot		机器人位姿
			ball		球位姿 	
**************************************************************************/
dbPOINT CDecisionMakingx::GetMidPoint(dbROBOTPOSTURE* pRobotInford,BallInformation ball,dbPOINT shoot_target, double deta_y)
{
	double		dx,dy;
	double	    dist;
	double		theta_br;	
	double		theta_gb;
	double		theta_e;
	double  	theta1,theta2;
	dbPOINT     target;
///////////////////////////////////////////
	dy = pRobotInford->y - ball.y;
	dx = pRobotInford->x - ball.x;
	//求球与机器人的距离
	dist = sqrt(dx*dx + dy*dy);
	target.x = ball.x; target.y = ball.y;
	//求球到机器人连线的方向角               
	theta_br = atan2(dy,dx);	 
	theta_br = cn_AngleTrim2PI(theta_br);

	dy = ball.y - shoot_target.y - deta_y;
	dx = ball.x - shoot_target.x;
	//求球到对方大门中点的角度
上一页 1 2 3 45
💿 文件大小 905 K
👤 上传用户 a415834839
📂 所属分类 VC书籍
🏷️ 相关标签

#仿真 #代码 #机器人
⌨️ 快捷键说明

复制代码 Ctrl + C
搜索代码 Ctrl + F
全屏模式 F11
切换主题 Ctrl + Shift + D
显示快捷键 ?
增大字号 Ctrl + =
减小字号 Ctrl + -