智能车电磁组——环岛处理

智能车电磁组——环岛处理

• • 一、电感排布
  • 二、圆环思路
  • • 1.入环
    • • （1）预圆环
      • （2）入环
    • 2.环岛内
    • 3.出环
    • • （1）预出环
      • （2）出环
  • 三、代码部分
  • • 1.入环
    • 2.圆环内寻迹
    • 3.出环

一、电感排布

二、圆环思路

1.入环

（1）预圆环

在电磁智能车中，圆环要想稳定入环，预圆环判断及处理是非常重要以及不可缺少的。

在预圆环的判断过程中，我使用的是M、L、M_2这三路电感，当进入靠近预圆环的位置时，M、L、M_2这三路电感都会增大，而且当到达预圆环的位置时，M电感与M_2电感会有一个差值，通过判断M、L、M-M_2大于阈值来判断预圆环的标志位。

（2）入环

当预圆环的标志位置1之后，接下来就到了入环，我是通过判断预圆环然后开始通过编码器去计路程，当路程达到一个定值之后就通过舵机固定打角，执行入环程序。

注：在编码器计路程的过程中，为了防止高速情况下因为电感值的变化过大导致舵机小幅度的摆动进而导致车身姿态不稳，可以在这过程中让差比和的误差手动置零，让舵机保持在中值状态。

2.环岛内

当执行入环程序之后，在圆环里不能一直保持固定打角，所以我在入环之后，开启陀螺仪的积分标志位，当陀螺仪的积分数值大于一个定值，则代表入环程序结束，执行环内寻迹。

这里环内寻迹我切换到第二排的三个电感寻迹，因为第一排的三个电感已经在赛道之外了，无法计算出正确的差比和值。
！！！！环内寻迹别忘了写丢线！！！！！！

3.出环

（1）预出环

预出环也是必不可少的一部分，因为如果正常寻迹接近到出环那里，与出环方向相反的那个电感的电感值会变得很大，经过差比和计算，舵机会打向和出环方向相反的方向。这也进而会导致车声冲出赛道。我的处理方案是陀螺仪的积分数值大于一个定值，就执行舵机固定打角。

（2）出环

预出环判定到后就执行出环程序，给舵机固定打角，当陀螺仪的积分值达到整整一圈之后，舵机回正，差比和手动置零。这时候如果让它正常寻迹的话，车子可能会因为还没远离圆环，导致左边或右边的电感值偏大，导致舵机往左大或者往右打，进而影响车身姿态。所以在出环执行完后，让车子自行一段距离，直到远离圆环，最后把圆环标志位、陀螺仪积分执行标志位和陀螺仪积分数值置零。等待下一次圆环。

！！！！！圆环的整个过程中，陀螺仪积分都在执行，直到圆环整个过程结束！！！！！

三、代码部分

1.入环

预圆环判断

if(M>650&&M-M_2>100&&L>500&&Statu == 1){
		L_RING_FLAG1=1;
		Statu=2;  //圆环状态
		BeeOn;		//蜂鸣器
		ring_running_flag = 1; //圆环内标志
		Record_Dis = 1;//开启计算距离
	}

入环

	if((L_RING_FLAG1 || L_RING_DELAY_FLAG1)){
		L_RING_FLAG1=0;
		L_RING_DELAY_FLAG1=1;
		if(Distance>75){//入环，路程大于一个值
			L_RING_DELAY_FLAG1=0;
			L_RING_FLAG2=1;
			Record_Angle=1; //开启陀螺仪积分
			Statu = 3;
		}
	}

结束入环
如果陀螺仪积分值大于一个阈值，则代表车身已在环内，代表入环结束，反之，则代表车身还没有在圆环内，继续执行入环。

if(L_RING_FLAG2 || L_RING_DELAY_FLAG2){
		L_RING_FLAG2=0;
		L_RING_DELAY_FLAG2=1;
		if(Angle>150){//陀螺仪积分到一定数值，代表车身已经在圆环内
			L_RING_DELAY_FLAG2=0;
			L_RING_FLAG3=1;
			Statu = 4;
			Record_Dis = 0;//距离清零，方便下次计数
			Distance = 0;
			
			//BeeOff;
		}
		else{
			pwm_duty(PWMB_CH1_P74,1490);	//舵机固定打角
		}
	}

2.圆环内寻迹

圆环内寻迹，则使用第二排的三个电感进行差比和运算。

if(ring_running_flag==1)
	{
			cha_2 = my_sqrt(a_L_2)-my_sqrt(a_R_2);
			he_2 = my_sqrt(a_L_2)+my_sqrt(a_M_2)+my_sqrt(a_R_2);
			bi_2	= 	cha_2/he_2;
			poserror = (int16)(bi_2*100);
	}

3.出环

预出环判定

	if(L_RING_FLAG3 || L_RING_DELAY_FLAG3){
		L_RING_FLAG3=0;
		L_RING_DELAY_FLAG3=1;
		if(Angle>735){//判定出还口，angle到达850代表跑了接近一圈
			L_RING_DELAY_FLAG3=0;
			L_RING_FLAG4=1;
			Statu = 5;
		}
		else
		{
			 cal_poserror(L,M,R,L_2,M_2,R_2);
		}
	}

执行出环，舵机固定打角

if(L_RING_FLAG4 || L_RING_DELAY_FLAG4){
		L_RING_FLAG4=0;
		L_RING_DELAY_FLAG4=1;
			if(Angle>1050){//快要结束圆环
			L_RING_DELAY_FLAG4=0;
			L_RING_FLAG5=1;
			Statu = 6;
		}
		else{
			pwm_duty(PWMB_CH1_P74,1500);	//舵机固定打角
		}
	}

出环后，差比和置零，车子直行，如果中间电感小于一个值，代表远离圆环，这时候圆环过程全部结束，正常寻迹和圆环标志位清零。

if(L_RING_FLAG5 || L_RING_DELAY_FLAG5){
		L_RING_FLAG5=0;
		L_RING_DELAY_FLAG5=1;
					if(M>580&&M<880){//
			L_RING_DELAY_FLAG5=0;
			Angle=0;
			Record_Angle=0;
			Statu=1;
			h=0;
			BeeOff;
			ring_running_flag=0;
		}
		else{
			cal_poserror(L,M,R,L_2,M_2,R_2);
		}
	}