这仅能使车以极慢的速度完成全元素任务

前提条件：能够

舵机打角

电机任意转速转动

读取电感前瞻获取到的值

屏幕显示

数据存储与读取

按键菜单

会更好：

陀螺仪数值读取

激光测距

干簧管/霍尔检测

编码器数据读取

速度闭环

在初始化里放各器件及函数的初始化

在中断里

	
		Cnt++; 
        KeyScanInt();
		Beep_IntCnt();
		Car_Protect();	
  //--------------------------电磁处理和元素判断-------------------------------//
		TOFGetData();//获取激光测距的值
		Inductor_deal();//获取前瞻电感值及滤波归一化等
		Ele_judge();//元素识别与识别后的处理
  //----------------------------陀螺仪数值获取---------------------------------//
	if(Cnt% 5== 0 ) {
		ICM20602DataFilter();//陀螺仪获取数据
		ICM_I_Int();//陀螺仪积分
	}
	
	if(!StartCar){
  //-------------------------------方向控制------------------------------------//
	if(Cnt% 5== 0) {
		DirectionControl();//舵机打角
	}
  //-------------------------------速度控制------------------------------------//
		ExSpeedControl();//顺序不能错
		DiffSpeedControl();
		SpeedControl();//电机控制
	}

对于元素判断

在车速很低的情况下

可以用模板匹配：

在发车前，手动采集元素入口的电感值

（用eeprom配合按键菜单，存储各个电感的值，标记为该元素）

还可以加上激光测距等辅助条件

#define value 1234
 
if (sqr(LNow[0]-Lreserve[0])+sqr(LNow[1]-Lreserve[1])+sqr(LNow[2]-Lreserve[2])+sqr(LNow[3]-Lreserve[3])+sqr(LNow[4]-Lreserve[4])+sqr(LNow[5]-Lreserve[5])+sqr(LNow[6]-Lreserve[6])+sqr(LNow[7]-Lreserve[7])
<=value)
{
fork_flag=1;
fork_state=1；
}

元素处理

判到元素之后，设置舵机及电机的变化