decisionmakingx.cpp

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

		if(dy > 0)
		{
			pLRWheelVelocity->LeftValue = k1*m_MoveParameter.V_max;
			pLRWheelVelocity->RightValue = k1*m_MoveParameter.V_max;
		}
		else
		{
			pLRWheelVelocity->LeftValue = -k1*m_MoveParameter.V_max;
			pLRWheelVelocity->RightValue = -k1*m_MoveParameter.V_max;
		}
	}
	else if(dx > 1.0 && dy >= 0)//第一象限
	{
		pLRWheelVelocity->LeftValue = k1*m_MoveParameter.V_max;
		pLRWheelVelocity->RightValue = cos(pi/2 - theta)*k1*m_MoveParameter.V_max;
	}
	else if(dx < 1.0 && dy >= 0)//第二象限
	{
		pLRWheelVelocity->LeftValue = cos(theta - pi/2)*k1*m_MoveParameter.V_max;
		pLRWheelVelocity->RightValue = k1*m_MoveParameter.V_max;
	}
	else if(dx < 1.0 && dy < 0)//第三象限
	{
		pLRWheelVelocity->LeftValue = -cos(theta + pi/2)*k1*m_MoveParameter.V_max;
		pLRWheelVelocity->RightValue = -k1*m_MoveParameter.V_max;
	}
	else//第四象限
	{	
		pLRWheelVelocity->LeftValue = -k1*m_MoveParameter.V_max;
		pLRWheelVelocity->RightValue = -cos(theta + pi/2)*k1*m_MoveParameter.V_max;
	}

	return 0;
}
//以优余弦曲线到定点
int CDecisionMakingx::s_Move2Pt(dbROBOTPOSTURE* pROBOTPOSTURE,dbPOINT Target,dbLRWheelVelocity* pLRWheelVelocity)
{
	double theta;
	double dx,dy,dx1,dy1;
	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(theta < 0) theta = pi/2 - theta;
	else theta = pi/2 - theta;
	if(theta > pi) theta = theta - 2*pi;
	if(fabs(theta) > m_AngleParameter.MaxAngle)
	{
		s_TurnToPointPD(pROBOTPOSTURE,Target,pLRWheelVelocity);
		return 0;
	}
	if(fabs(theta) < m_AngleParameter.AngleError)
	{
		pLRWheelVelocity->LeftValue = m_MoveParameter.V_max;
		pLRWheelVelocity->RightValue = m_MoveParameter.V_max;
	}
	else if(theta > 0 )
	{
		pLRWheelVelocity->LeftValue = m_MoveParameter.V_max;
		pLRWheelVelocity->RightValue = cos(theta)*m_MoveParameter.V_max;
	}
	else if(theta < 0)
	{
		pLRWheelVelocity->RightValue = m_MoveParameter.V_max;
		pLRWheelVelocity->LeftValue = cos(theta)*m_MoveParameter.V_max;
	}
	return 0;
}
//以余弦曲线且按给定的速度到定点
int CDecisionMakingx::Move2Pt(dbROBOTPOSTURE* pROBOTPOSTURE,dbPOINT Target,double speed,dbLRWheelVelocity* pLRWheelVelocity)
{
	double theta,temp_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(dx) < 1)
	{
		if(dy > 0)
		{
			pLRWheelVelocity->LeftValue = k1*speed;
			pLRWheelVelocity->RightValue = k1*speed;
		}
		else
		{
			pLRWheelVelocity->LeftValue = -k1*speed;
			pLRWheelVelocity->RightValue = -k1*speed;
		}
	}
	else if(dx > 1 && dy >= 0)//第一象限
	{
		temp_theta = pi/2 - theta;
		if(temp_theta < pi/7) temp_theta *= 2;
		pLRWheelVelocity->LeftValue = k1*speed;
		pLRWheelVelocity->RightValue = cos(temp_theta)*k1*speed;
	}
	else if(dx < -1 && dy >= 0)//第二象限
	{
		temp_theta = theta - pi/2;
		if(fabs(temp_theta) < pi/7) temp_theta *= 2;
		pLRWheelVelocity->LeftValue = cos(temp_theta)*k1*speed;
		pLRWheelVelocity->RightValue = k1*speed;
	}
	else if(dx < -1 && dy < 0)//第三象限
	{
		temp_theta = theta + pi/2;
		if(fabs(temp_theta) < pi/7) temp_theta *= 2;
		pLRWheelVelocity->LeftValue = -cos(temp_theta)*k1*speed;
		pLRWheelVelocity->RightValue = -k1*speed;
	}
	else//第四象限
	{	
		temp_theta = theta + pi/2;
		if(fabs(temp_theta) < pi/7) temp_theta *= 2;
		pLRWheelVelocity->LeftValue = -k1*speed;
		pLRWheelVelocity->RightValue = -cos(temp_theta)*k1*speed;
	}
	return 0;
}
int CDecisionMakingx::s_Move2Pt(dbROBOTPOSTURE* pROBOTPOSTURE,dbPOINT Target,double speed,dbLRWheelVelocity* pLRWheelVelocity)
{
	double theta;
	double dx,dy,dx1,dy1;
	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(theta < 0) theta = pi/2 - theta;
	else theta = pi/2 - theta;
	if(theta > pi) theta = theta - 2*pi;
	if(fabs(theta) > m_AngleParameter.MaxAngle)
	{
		s_TurnToPointPD(pROBOTPOSTURE,Target,pLRWheelVelocity);
		return 0;
	}
	if(fabs(theta) < m_AngleParameter.AngleError)
	{
		pLRWheelVelocity->LeftValue = speed;
		pLRWheelVelocity->RightValue = speed;
	}
	else if(theta > 0 )
	{
		pLRWheelVelocity->LeftValue = speed;
		pLRWheelVelocity->RightValue = cos(theta)*speed;
	}
	else if(theta < 0)
	{
		pLRWheelVelocity->RightValue = speed;
		pLRWheelVelocity->LeftValue = cos(theta)*speed;
	}
	return 0;
}
//到定点,PD算法
int CDecisionMakingx::ToPositionPD(dbROBOTPOSTURE* pROBOTPOSTURE,dbPOINT Target,double same_speed, double end_speed,dbLRWheelVelocity* pLRWheelVelocity)
{ 
	if(same_speed>35)same_speed=35;//为追求高速，由张毅添加45


	//vBase是使小车到达定点时保持一定的速度
	int clock_sign,move_sign;
	double theta,theta_e1;//e1为当前角度误差
	static double theta_e2 = 0;//e2为上一周期角度误差
	double dx,dy,dx1,dy1,distance;
	double 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_distance)
		speed = same_speed;
	else 
		speed =  distance/m_MoveParameter.max_distance*same_speed;
	if(speed < end_speed) speed = end_speed;
	theta = atan2(dy,dx);	
	if(fabs(fabs(theta) - pi/2) > m_AngleParameter.MaxAngle)
	{
		TurnToPointPD(pROBOTPOSTURE,Target,NOCLOCK,pLRWheelVelocity);
		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;
	}

	pLRWheelVelocity->LeftValue = speed*move_sign + clock_sign*(m_MoveParameter.kp4pospd*theta_e1 + m_MoveParameter.kd4pospd*(theta_e1- theta_e2));
	pLRWheelVelocity->RightValue = speed*move_sign - clock_sign*(m_MoveParameter.kp4pospd*theta_e1 + m_MoveParameter.kd4pospd*(theta_e1- theta_e2));
	
	//保存本周期角度误差，下一周期作微分用	
	theta_e2 = theta_e1;
	return 0;
}
int CDecisionMakingx::s_ToPositionPD(dbROBOTPOSTURE* pROBOTPOSTURE,dbPOINT Target,double same_speed,double end_speed,dbLRWheelVelocity* pLRWheelVelocity)
{                                                                             //vBase是使小车到达定点时保持一定的速度
	int clock_sign;
	double theta,theta_e1;//e1为当前角度误差
	static double theta_e2 = 0;//e2为上一周期角度误差
	double dx,dy,dx1,dy1,distance;
	double 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_distance)
		speed = same_speed;
	else 
		speed =  distance/m_MoveParameter.max_distance*same_speed;
	if(speed < end_speed) speed = end_speed;
	theta = atan2(dy,dx);	
	if(theta < 0) theta = pi/2 - theta;
	else theta = pi/2 - theta;
	if(theta > pi) theta = theta - 2*pi;
	if(fabs(theta) > m_AngleParameter.MaxAngle)
	{
		s_TurnToPointPD(pROBOTPOSTURE,Target,pLRWheelVelocity);
		return 0;
	}
	if(theta > 0)	clock_sign = 1;
	else clock_sign = -1;
	theta_e1 = fabs(theta);
	pLRWheelVelocity->LeftValue = speed + clock_sign*(m_MoveParameter.kp4pospd*theta_e1 + m_MoveParameter.kd4pospd*(theta_e1- theta_e2));
	pLRWheelVelocity->RightValue = speed - clock_sign*(m_MoveParameter.kp4pospd*theta_e1 + m_MoveParameter.kd4pospd*(theta_e1- theta_e2));
	//保存本周期角度误差，下一周期作微分用	
	theta_e2 = theta_e1;
	return 0;
}
/************************************************************************************
函数名	：ToPositionNew(robotInformation &robot, point targetpt, ballInformation &ball, float samespeed, BOOL IfEndprocess)
创建日期：19/4/2001
作者	：
功能	：到下个位姿
参数	：	robot					机器人位姿
			targetpt				目标点
			ball					球
			samespeed				同向速度
************************************************************************/

int CDecisionMakingx::ToPositionNew(dbROBOTPOSTURE* robot, ballInformation ball,dbPOINT directionpt, double samespeed, int IfEndprocess,dbLRWheelVelocity* pSpeed)
{
	if(samespeed > 30)samespeed=30;//为追求高速，由张毅添加45

	int vemax;
	vemax = 110;
	double dist;
	double Dx,Dy;
	double Anglerb2target;
	
		dbPOINT	ballPt;
		double delta,angletemp;
		delta = 6.5;
		ballPt = ball;
		angletemp = Getpt2ptAngle(ballPt,directionpt);
		//ball.x = ball.x - delta*sin(angletemp);
		//ball.y = ball.y + delta*cos(angletemp);

	robot->y = walltop - robot->y;
	robot->theta = 2*pi - robot->theta;
	ball.y = walltop - ball.y;
	directionpt.y = walltop - directionpt.y;
	robot->theta = cn_AngleTrim2PI(robot->theta);
	//计算机器人到目标点的角度
	Dx = ball.x - robot->x;Dy = ball.y - robot->y;
	dist = sqrt(Dx*Dx+Dy*Dy);


	double kp4new;

	kp4new = 10;//

	Anglerb2target = atan2(Dy ,Dx);
	Anglerb2target = cn_AngleTrim2PI(Anglerb2target);
	//计算targetpt到directionpt的角度
	double Angletpt2dpt;
	Dx = directionpt.x - ball.x;Dy = directionpt.y - ball.y;
	Angletpt2dpt = atan2(Dy ,Dx);
	Angletpt2dpt = cn_AngleTrim2PI(Angletpt2dpt);
	//计算两者的差值
	double angle;
	angle = Angletpt2dpt - Anglerb2target;
	angle = cn_AngleTrim2PI(angle);
	if(angle >pi)
		angle -=2*pi;
	double sign;
	if(angle>0)
		sign =1;
	else 
		sign =-1;
	//计算下个周期的目标角度
	double disiredAngle;
	disiredAngle = Anglerb2target - sign*TNpara(IfEndprocess,*robot,ball,directionpt,distRobot2Pt(*robot,ball),fabs(angle));
	disiredAngle = cn_AngleTrim2PI(disiredAngle);
	//计算角度偏差
	double Angle_e;
	Angle_e = disiredAngle - robot->theta;
	Angle_e = cn_AngleTrim2PI(Angle_e);
	
	double ka,la;
	la = pi*.45;
	if(robot->x<7||robot->x>wallright-7||robot->y<7||robot->y>walltop-7)
		la = pi*.27;
	ka=samespeed;
	double speed_e;
	if(Angle_e <= pi/2)//角度偏差在第一象限
	{
		speed_e = kp4new*Angle_e;
		if(fabs(Angle_e)>la)
			samespeed = 0;
		else
			samespeed = ka*(pi/2-fabs(Angle_e))/la;

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

		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);
		if(fabs(Angle_e - pi)>la)
			samespeed = 0;
		else
			samespeed = ka*(pi/2-fabs(Angle_e - pi))/la;

		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);
		if(fabs(2*pi - Angle_e)>la)
			samespeed = 0;
		else
			samespeed = ka*(pi/2-fabs(2*pi - Angle_e))/la;

		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);
	}
	//修改成自然坐标系
	ball = ballPt;
	robot->y = walltop - robot->y;
	robot->theta = 2*pi - robot->theta;
	//ball.y = 180 - ball.y;
	directionpt.y = walltop - directionpt.y;
	robot->theta = cn_AngleTrim2PI(robot->theta);
	EndProcess(IfEndprocess,robot,directionpt,ball,pSpeed);	
	return 1;
}
//
int CDecisionMakingx::ToPositionN(dbROBOTPOSTURE* robot,dbPOINT directionpt, double samespeed, dbLRWheelVelocity* pSpeed)
{
	if(samespeed!=20) samespeed=gVMAX;//为追求最大速度，由张毅添加
	if(samespeed >50)samespeed = 45;//45
	int vemax;
	vemax = 110;
	double dist;
	double Dx,Dy;
	double Anglerb2target;
	robot->y = walltop - robot->y;
	robot->theta = 2*pi - robot->theta;
	directionpt.y = walltop - directionpt.y;
	robot->theta = cn_AngleTrim2PI(robot->theta);