angle.cpp

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

void Angle( Environment *env, int robot,double angle)
{

	Mydata * p;
	p=(Mydata *)env->userData;

	double speed = 0;		//和pangle接轨
	double accuracy=1;
	double turnangle=0,nextangle=0;
	double FF=125;		//最大减速度
//FF为左右轮速差的一半
	turnangle = angle -p->robot[robot].rotation;
	RegulateAngle(turnangle);
//根据目标角度和当前角度的差来选择适当的左右轮速的差  FF
	if(turnangle < 1 && turnangle >-1)
	{
		Velocity(env,robot,0,0);
		return ;
	}
	else if(turnangle < 2 && turnangle >-2)
		FF=10;
	else if( turnangle >-3 && turnangle < 3)
		FF=15;
	else if( turnangle >-5 && turnangle < 5)
		FF=30;

	double v=p->robot[robot].rotation - p->myoldpos[robot].z ;
//v是当前robot的转动角速度
	RegulateAngle(v);
	double v1=v;
	double f=0;	//相当于减速时,右轮速度，
//	int n=0;
	bool turnleft=true;			//判断小车是否是该向左转
	double a=ANGLE_A;
	double b=ANGLE_B;
//因为robot的头尾是等效的，所以我们应该判断是头更接近目标角度还是尾更接近目标角度
	if(turnangle > 90)
	{
		turnleft=false;
		turnangle-=180;
	}
	else if(turnangle >0)
	{
		turnleft=true;	
	}
	else if(turnangle > -90)
	{
		turnleft=false;	
	}
	else 
	{
		turnleft=true;
		turnangle+=180;		
	}

	if(turnleft)
	{//
		f=-FF;

		v1=AngleOne(v1,speed+f,speed-f);		//v1+=a *( -b *f-v1);
		nextangle+=v1;
//计算如果按最大加速度转动后robot的角度

		do{//whether to reduce
			//收敛!!
			v1 =AngleOne(v1,speed-f,speed+f);//+= a *( b *f-v1);		// v1   
			nextangle+=v1;
		}while( v1 > 0  );
//这个循环的是模拟让robot以最大减速度减速到转动开始反向是转动过的角度
//下面将会拿这个角度和目标角度比较如果不超过目标角度，则可以继续以最大速度加速旋转
		nextangle-=v1;

		if(nextangle < turnangle)
		{//不满足减速条件 所以 f 取相反数
			Velocity(env,robot,speed+f,speed-f);
		}
		else 
//这时，如果再继续加速旋转的话，则来不及“刹车”，因此要开始刹车减速了
		{//reduce
	
			v1 = AngleOne(v,speed-f,speed+f);  //v + a *( b *f-v);
//选取适当的刹车力度ff，使得robot刚好不超过设定的方向
			if( v1 < 0 )
			{
				do{//该降低功率了
					f++;
					v1 = AngleOne(v,speed-f,speed+f);  //v + a *( b *f-v);
				}while( v1 < turnangle && f <FF);
			}
			Velocity(env,robot,speed-f,speed+f);
		}		
	}
	else
	{//同上
		f=FF;
		v1=AngleOne(v1,speed+f,speed-f);		//v1+=a *( -b *f-v1);
		nextangle+=v1;
		
		do{//whether to reduce
			v1 =AngleOne(v1,speed-f,speed+f);//+= a *( b *f-v1);		// v1   
			nextangle+=v1;
		}while( v1 < 0 );
		
		nextangle-=v1;

		if(nextangle > turnangle)
		{//不满足减速条件 所以 f 取相反数
			Velocity(env,robot,speed+f,speed-f);
		}
		else 
		{//reduce
			v1 = AngleOne(v,speed-f,speed+f);  //v + a *( b *f-v);
			if( v1 > 0 )
			{
				do{//该降低功率了
					f--;
					v1 = AngleOne(v,speed-f,speed+f);  //v + a *( b *f-v);
				}while( v1 > turnangle && f >-FF);
			}
			Velocity(env,robot,speed-f,speed+f);
		}		
	}

}