position.cpp

机器人足球的基本动作函数

void PositionAndStop(Environment *env,int  robot,Vector3D pos ,double bestangle,double limit) 
{	//考虑到可能的	急停和 急快速加速
	//特别作了优化
	//还有就是 被碰转后的转角过程 不能耽搁时间!!!
	//转角是最危险的过程

	Mydata * p;
	p=(Mydata *)env->userData;
	
	double anglespeedmax=0;		//控制转交速度的变量
	double vmax=125;			//默认的跑位加速度
	double Limitedangle=2;		//默认减速范围
//limit 的 含义是： 目标点应该是一个范围，而limit就是这个范围的半径
	if( limit < 0.5 )
		limit =0.5;
	double Limiteddis=limit;	//减速范围有一个下限，保证不会来回跑动
	
	double  distance;			//robot和目标点的距离
	double turnangle,posangle,vangle;	//转动角度 ，目标点相对robot的角度，速度的绝对角度
	double dx,dy;				//pos  和robot的坐标差
	double a=SPEED_A;			//参数
	double b=SPEED_B;
	double v,v1;				//临时保存速度的大小!!!
	double f=vmax;				//加速度变量
	double s=0;					//预测的减速位移(路程)
	int n=0;					//跑位的步数
	bool face=true;			//判断小车是否是正面前进

	v= sqrt(p->myspeed[robot].x * p->myspeed[robot].x + p->myspeed[robot].y*p->myspeed[robot].y);
	//临时保存速度的大小!!!
	dx = pos.x - p->robot[robot].pos.x ;		//pos  和robot的坐标差
	dy = pos.y - p->robot[robot].pos.y ;
	
	distance = Distance(p->robot[robot].pos , pos);
	posangle = Atan(dy,dx);

	turnangle = p->robot[robot].rotation - posangle;		//转动角度 
	RegulateAngle(turnangle);

//考虑是正面跑还是倒退，因为robot的头尾是等效的
	if(turnangle > 90)
	{//判断小车是否是正面前进
		face=false;
		turnangle-=180;
	}
	else if(turnangle < -90)
	{
		face=false;
		turnangle+=180;
	}
	else 
	{
		face=true;
	}

	vangle = p->myspeed[robot].z - p->robot[robot].rotation;		//速度的方向和robot正面的夹角
	RegulateAngle(vangle);					//主要用于最后控制减速度的大小
	if( vangle <-90 || vangle > 90 )		//同时判断v的正负
		v=-v;
	
//下面判断robot运动是否远离目标点
	if(face)
	{//forward	跑位，如果后退的话  就v=0
		//设vl,vr=0 还是vl,vr=125 有一个条件有一个临界条件那就是 
		//v = SPEED_ZERO
		if(v < -SPEED_ZERO)
		{
			Velocity(env,robot,0,0);
			return ;
		}
	}
	else if(v > SPEED_ZERO)
	{//back		跑位，如果后退的话  就v=0
		Velocity(env,robot,0,0);
		return ;
	}

	v1=v;	//v1 is changing while program running 
			//whlie, v is not
//Limiteddis：大于Limiteddis时，不考虑减速
	if(distance > Limiteddis )
	{//it is too early to count the steps
		//but the Limiteddis should be tested!!	to do...
		if(turnangle > Limitedangle || turnangle < -Limitedangle)
		{//adjust angle
			/////////////////测试这一段
			//对于goalie这一段应该特别注意
			//发生变向	1.knock the robot,especially the opponent
			//	2.knock the wall
			// so the anglespeedmax is allowed ++ more!!
			if(turnangle > 20  || turnangle < -20)
				anglespeedmax = 0;
			else if(turnangle > 10  || turnangle < -10)
				anglespeedmax = 125;
			else if(turnangle > 5  || turnangle < -5)
				anglespeedmax = 180;
			else 
				anglespeedmax = 200;
			///////////////测试这一段
			PAngle(env,robot,posangle,anglespeedmax);	
		}
		else
		{
			if(face)
				Velocity(env,robot,f,f);
			else
				Velocity(env,robot,-f,-f);
		}//it is time to rush
	}
	else	//开始考虑角度的偏差了
	{
		if(distance > 1)
		{		//调整角度	return!!!!!!
			//radious of robot is about 1.5 ,so the distance is very short
			if(turnangle > Limitedangle || turnangle < -Limitedangle)
			{	
				Angle(env,robot,posangle);
				return ;
			}
		}

		if(distance < 0.4)
		{	//停止并转向		return!!!!!!
			//radious of robot is about 1.5 ,so the distance is very short
			if( v<0.1 && v>-0.1)
			{	//the range of v should be tested 
					Angle(env,robot,bestangle);
				return	;
			}
		}
//如果不需要调整角度，那么考虑减速！
//跑位函数的减速过程和Angle是一样的，所以不再赘述
		if(true)
		{
			vmax=125;
			if(face)
			{
				f=-vmax;		//减速度  为  0000000
				v1=VelocityOne(v1,-f,-f);		//加速一步
				s=v1;
				do{//whether to reduce
//之所以有与SPEED_ZERO比较，是因为v1=VelocityOne(v1,0,0)刹车和v1=VelocityOne(v1,f,f)效果不同

					if(v1 > SPEED_ZERO)	//as i said,this is limited
						v1=VelocityOne(v1,0,0);
					else 
						v1=VelocityOne(v1,f,f);
					s+=v1;
				}while( v1 > 0 );

				s-=v1;

				if(s < distance)
				{//不满足减速条件加速
					Velocity(env,robot,-f,-f);
				}
				else
				{
					if(v > SPEED_ZERO)
						Velocity(env,robot,0,0);
					else
					{
						v1=VelocityOne(v,f,f);		//减速一步
						if( v1 < 0 )
						{
							do{//该降低功率了
								f++;		//f=-vmax;
								v1 = VelocityOne(v,f,f);
							}while( v1 < distance && f < vmax);	
						}
						Velocity(env,robot,f,f);
					}
				}
			}			
			else
			{
				f=vmax;		//减速度!!!!!
				v1=VelocityOne(v1,-f,-f);
				s=v1;
				do{//whether to reduce
					if(v1 < -SPEED_ZERO)	//as i said,this is limited
						v1=VelocityOne(v1,0,0);
					else 
						v1=VelocityOne(v1,f,f);
					s+=v1;
				}while( v1 < -0.1 );

				s-=v1;

				if(s > -distance)
				{//不满足减速条件加速
					Velocity(env,robot,-f,-f);
				}
				else
				{
					if(v < -SPEED_ZERO)
						Velocity(env,robot,0,0);
					else
					{
						v1=VelocityOne(v,f,f);		//减速一步
						if( v1 > 0 )
						{
							do{//该降低功率了
								f--;		//f=-vmax;
								v1 = VelocityOne(v,f,f);
							}while( v1 > -distance && f > -vmax);	
						}
						Velocity(env,robot,f,f);
					}
				}
			}	
		}
	}
}