飞思卡尔智能车电磁组分区算法介绍 - 写代码的汪

时间:2024-03-02 15:44:17

飞思卡尔智能车电磁组分区算法介绍

写在之前的话:

1、目前我是一名在校学生,这也是我第一次写博客,不周之处,请多谅解;

2、此算法并非原创,借鉴自山东德州学院第八届白杨队(PS:个人看法,对于一些人把别人的开源东西改头换面一下就说是自己的原创行为十分鄙视);

3、对于此算法的理解和说明并非纸上谈兵,算法已经被我运用到了小车比赛中并取得好的成绩(具体就不多说了,比赛时车莫名其妙坏了,比赛前调试的速度绝对能进国赛,比较遗憾),总之这算法是我尝试过的最好的算法;

4、这一次所介绍的只是路径算法和一些知识普及,后面有时间会介绍其余部分算法及许多好的思路(舵机电机控制思路(不只是简单的PID),双车策略);

5、希望对于这方面有涉及的人能与我联系并交流或指出不足之处。

6、转载请说明出处:http://www.cnblogs.com/roc-wwp/p/4848136.html(PS:看到有人直接改成恩智浦智能车,也是醉了,这是我上学时写的,当时好自信啊,2333)

---------------------------------------------------------------分割线-----------------------------------------------------------------------------

一、没有这方面了解的可以看看

  飞思卡尔智能车分为三组:摄像头、光电、电磁,我做的是电磁车,三种车队区别在于传感器的不同,所以获得路径信息的方法也不一样,摄像头和光电识别的是赛道上的黑线(白底赛道),而电磁车则是检测埋在赛道下的通入100mh电流的漆包线,摄像头和光电采用的是摄像头和ccd作为传感器,电磁则是用电感放在漆包线周围,则电感上就会产生感应电动势,且感应电动势的大小于通过线圈回路的磁通量成正比,又因为漆包线周围的磁感应强度不同,因此不同位置的电感的感应电动势就不同,因此就可以去确定电感位置;因此在车子前面设置了50cm的前瞻,电感布局如下(怎么发不了图片):分为两排,前排3个,编号0,1,2(前期还加了两个竖直电感用来帮助过直角弯,后来改为了八字电感);后排2个,编号3,4;现在车子获得了不同位置的感应电动势的大小了,但这些值是不能处理的:1、感应电动势太微弱;2、是模拟信号,信号太微弱就放大它;这就涉及到模拟电路的知识了,就不多说了(因为要把这讲完到PCB绘制的篇幅就足够写另开一号专门写这些方面来(PS:题外话(我的题外话比较多)):放大部分外围你设计的再好也抵不过一个更好的芯片,有两个例子,一个是我自己的:之前用的是NE5532,但是效果不理想,加了好多什么滤波,补偿,都用上,没用,软件里处理后面再说,后来一狠心换了AD620,感觉像是春天来了,因为它是仪用放大器,还有就是贵。。。,效果是超级赞,去掉了许多附加电路,板子简洁多了,(器件多,故障率也会提高,到后期查故障简直是恶心(能想象拿着万用表和示波器在实验室纠结的样子吗?感觉怎样处理故障还可以专门总结一下))另一个是学长的例子:他参加电子设计竞赛时,有一组(同一学校的)和他题目一样,东西做出来后基本差不多,但是他拿了一等奖,而另一组是二等奖,差别就在于当时的一个滤波,本校指导老师说可以加一个简单电容就行,而他用的则是一款新的滤波芯片,信号最后出来的波形差距挺大的),(我负责的主要是软件部分,但是硬件电路也全程涉及了,不过终究还是不太熟)。

  解决完放大就是把模拟信号转换为数字信号了,我们直接用的是芯片上的AD模块,(题外话又来啦:我开始用的是K60是飞思卡尔Kinetis微控制器ARM Cortex®-M4系列,其实就是把它当成了一款高性能的单片机了,实在是浪费,(现在在学嵌入式开发,希望有高手来指导我),后来改为s12了,因为与别的组冲突,感觉K60确实比s12更容易上手,s12还要自己配置寄存器,级联,计算超频,外部时钟频率,k60别人写好了库,就是在不断调用),不过看论坛上有人说飞思卡尔的AD转换有问题,而飞思卡尔也不公开它AD模块具体细节(这貌似有点心虚了,呵呵),不过要是自己做ad模块,那对于整车布局就十分麻烦,所以只好用啦(别人都在用,一起跳坑);处理完了,接下来关门放算法。

二、核心路径算法(为致敬开源,放的是原作者算法)

 

  1 /******************** (C) COPYRIGHT 2011 ********************* ********************
  2  * 文件名       :Date_analyse.c
  3  * 描述         :电感数据采集与分析
  4  *
  5  * 实验平台     :野火kinetis开发板
  6  * 库版本       :
  7  * 嵌入系统     :
  8  *
  9  * 作者         :oО殇のSo 
 10 **********************************************************************************/    
 11 #include "include.h"
 12 #define   NM    3
 13 
 14 int16  AD_valu[5],AD_V[5][NM],chazhi,chazhi_old;
 15 float  AD[5],sensor_to_one[5];
 16 float  Slope_AD_1;  // 用于坡道检测
 17 int16  max_v[5],min_v[5];  //电感标定 采集值
 18 int16  Position_transit[4];  //记录过渡点归一化的值
 19 int16  AD_sum[5]; 
 20 int16  AD_MAX_NUM;   //
 21 int16  position = 2,position_back = 1;
 22 float  max_value,AD_0_max,AD_1_max,AD_2_max,AD_3_max;
 23 
 24 
 25 /*************************************************************************
 26 *  函数名称   SC_black_Init
 27 *  功能说明: 最大值采样
 28 *  参数说明:         
 29 *  函数返回: 无
 30 *  修改时间:
 31 *  备    注:
 32 *************************************************************************/
 33 void SC_black_Init(void)
 34 {
 35    uint16  i,j;
 36    int16   Position_transit_short[4];
 37    float   sensor_1,sensor_2,sensor_3,sensor_4;
 38    if(K2)
 39    {
 40        LCD_Print(25,2,"Collecting"); 
 41        LCD_Print(28,4,"samples..."); 
 42    
 43        max_v[0] = max_v[1] = max_v[2] = max_v[3] = max_v[4] = 0;
 44        min_v[0] = min_v[1] = min_v[2] = min_v[3] = min_v[4] = 7;  
 45        for(i=0;i<1200;i++) 
 46        {
 47            AD_valu[0] = ad_ave(ADC1,AD9,ADC_10bit,6);  //PTC0  通道      
 48            AD_valu[1] = ad_ave(ADC1,AD8,ADC_10bit,6);  //PTC1  通道     
 49            AD_valu[2] = ad_ave(ADC1,AD15,ADC_10bit,6); //PTE25 通道     
 50            AD_valu[3] = ad_ave(ADC1,AD11,ADC_10bit,6); //PTE24 通道 
 51            AD_valu[4] = ad_ave(ADC1,AD13,ADC_10bit,6); //PTE24 通道 
 52            for(j=0;j<5;j++) 
 53            {      
 54                if(AD_valu[j] > max_v[j]) 
 55                {
 56                    max_v[j] = AD_valu[j];
 57                    if(j==0)    Position_transit_short[0] =  AD_valu[1];  //记录过渡点 电感值
 58                    if(j==2)    Position_transit_short[1] =  AD_valu[1];  
 59                    if(j==3)    Position_transit_short[2] =  AD_valu[4];  
 60                    if(j==4)    Position_transit_short[3] =  AD_valu[3];  
 61                }
 62            }
 63            delayms(1);           //延时    
 64        }
 65        /***************              记录的过渡点归一化                     ******************/
 66        sensor_1 = (float)(Position_transit_short[0] - min_v[1])/(float)(max_v[1] - min_v[1]); 
 67        if(sensor_1 <= 0.0)  sensor_1 = 0.001;
 68        if(sensor_1 >= 1.0)  sensor_1 = 1.0; 
 69        
 70        sensor_2 = (float)(Position_transit_short[1] - min_v[1])/(float)(max_v[1] - min_v[1]); 
 71        if(sensor_2 <= 0.0)  sensor_2 = 0.001;
 72        if(sensor_2 >= 1.0)  sensor_2 = 1.0; 
 73        
 74        sensor_3 = (float)(Position_transit_short[2] - min_v[4])/(float)(max_v[4] - min_v[4]); 
 75        if(sensor_3 <= 0.0)  sensor_3 = 0.001;
 76        if(sensor_3 >= 1.0)  sensor_3 = 1.0; 
 77        
 78        sensor_4 = (float)(Position_transit_short[3] - min_v[3])/(float)(max_v[3] - min_v[3]); 
 79        if(sensor_4 <= 0.0)  sensor_4 = 0.001;
 80        if(sensor_4 >= 1.0)  sensor_4 = 1.0; 
 81        
 82        Position_transit[0] = (int16)(100 * sensor_1);
 83        Position_transit[1] = (int16)(100 * sensor_2); 
 84        Position_transit[2] = (int16)(100 * sensor_3);
 85        Position_transit[3] = (int16)(100 * sensor_4); 
 86        
 87        
 88        flash_erase_sector(SECTOR_ADM);       //擦除254扇区    
 89        for(i=0; i<5; i++)                    //电感标定的最大值写入扇区
 90        { 
 91            flash_write(SECTOR_ADM,i*4,max_v[i]);     
 92        }  
 93        for(i=0;i<4;i++)                     //过渡点归一化值写入扇区 
 94        {
 95           flash_write(SECTOR_ADM,20+i*4,Position_transit[i]);  
 96        }
 97    }
 98    else 
 99    {
100        for(i=0;i<3;i++)
101        { 
102           for(j=0;j<5;j++)   //读取五个电感的采样标定的最大值
103           {
104               max_v[j] = flash_read(SECTOR_ADM,j*4,int16);
105           }
106           for(j=0;j<4;j++)  //读取过渡点
107           {
108              Position_transit[j] = flash_read(SECTOR_ADM,20+j*4,int16);
109           }
110             
111           LCD_Print(29,2,"Reading"); 
112           LCD_Print(28,4,"samples..."); 
113           delayms(10);           
114        }
115    }
116    LCD_CLS();             
117    Beer_ON; 
118    delayms(25);  
119    Beer_OFF;
120 }
121 
122 /*************************************************************************
123 *  函数名称   Read_ADC
124 *  功能说明: AD采集
125 *  参数说明:         
126 *  函数返回: 无
127 *  修改时间:
128 *  备    注:
129 *************************************************************************/
130 void Read_ADC(void)
131 {
132      int16  i,j,k,temp;
133      int16  ad_valu[5][5],ad_valu1[5],ad_sum[5];
134      
135      for(i=0;i<5;i++)
136      {
137          ad_valu[0][i]=ad_ave(ADC1,AD9,ADC_10bit,7);     // ADC0 通道 
138          ad_valu[1][i]=ad_ave(ADC1,AD8,ADC_10bit,7);     // ADC0 通道 
139          ad_valu[2][i]=ad_ave(ADC1,AD15,ADC_10bit,7);     // ADC0 通道
140          ad_valu[3][i]=ad_ave(ADC1,AD11,ADC_10bit,7);     // ADC0 通道 
141          ad_valu[4][i]=ad_ave(ADC1,AD13,ADC_10bit,7);     // ADC0 通道 
142      }
143   //////////////////////冒泡排序///////////////////////////////////
144      for(i=0;i<5;i++)     //5个电感     
145      {
146         for(j=0;j<4;j++)  //五个数据排序
147         {
148            for(k=0;k<4-j;k++)
149            {
150               if(ad_valu[i][k] > ad_valu[i][k+1])  //前面的比后面的大  则进行交换
151               {
152                  temp = ad_valu[i][k+1];
153                  ad_valu[i][k+1] = ad_valu[i][k];
154                  ad_valu[i][k] = temp;
155               }
156            }
157         }
158      }
159      for(i=0;i<5;i++)    //求中间三项的和
160      {
161         ad_sum[i] = ad_valu[i][1] + ad_valu[i][2] + ad_valu[i][3];       
162         ad_valu1[i] = ad_sum[i] / 3;
163      }
164   ////////////////////////滑动平均滤波/////////////////////////////
165      for(i = 0;i < NM-1;i ++)
166      {
167          AD_V[0][i] = AD_V[0][i + 1];
168          AD_V[1][i] = AD_V[1][i + 1];
169          AD_V[2][i] = AD_V[2][i + 1];
170          AD_V[3][i] = AD_V[3][i + 1];
171          AD_V[4][i] = AD_V[4][i + 1];
172      }
173      for(i=0;i<5;i++)
174      {
175          AD_V[i][NM-1] =  ad_valu1[i];
176      }
177     
178      for(i = 0;i < NM;i ++)
179      {
180          AD_sum[0] += AD_V[0][i];
181          AD_sum[1] += AD_V[1][i];
182          AD_sum[2] += AD_V[2][i];
183          AD_sum[3] += AD_V[3][i];
184          AD_sum[4] += AD_V[4][i];
185      }
186      for(i=0;i<5;i++)  //求平均
187      {
188          AD_valu[i] = AD_sum[i] / NM;
189          AD_sum[i] = 0;   
190      }    
191 }
192 
193 /*************************************************************************
194 *  函数名称   Date_analyse
195 *  功能说明: 数据分析
196 *  参数说明:         
197 *  函数返回: 无
198 *  修改时间:
199 *  备    注:
200 *************************************************************************/
201 void Date_analyse()
202 { 
203     int16  i,max_front=0,max_back; 
204     static int16 max_old = 1,max_crosstalk = 1;
205     static int16 position_last = 2;
206     float  sensor_1;
207     
208     Read_ADC();
209     
210     /*********************归一化处理********************/
211     for(i=0;i<5;i++) 
212     {
213        sensor_to_one[i] = (float)(AD_valu[i] - min_v[i])/(float)(max_v[i] - min_v[i]); 
214        if(sensor_to_one[i]<=0.0)  sensor_to_one[i]=0.001;
215        if(sensor_to_one[i]>1.0)  sensor_to_one[i]=1.0; 
216        
217        AD[i] = 100 * sensor_to_one[i];     //AD[i]为归一化后的值  范围为0-100
218     }      
219        /*******1号电感特殊归一化,用于坡道检测********/
220      sensor_1 = (float)(AD_valu[1] - min_v[1])/(float)(max_v[1] - min_v[1]);  
221      if(sensor_1 <= 0.0)  sensor_1 = 0.001;
222      Slope_AD_1 = 100 * sensor_1;
223   ////////////////////////////////////////////////////////////////////
224     
225       for(i=0;i<3;i++)                 //找出最强的传感器
226       {  
227         if(AD[max_front]<AD[i]-2)
228           max_front=i;
229       } 
230       max_value=AD[max_front]; 
231       
232       max_back = (AD[3]>AD[4])? 3:4;  //找后排最强电感
233       
234       if(max_value < 43)            //丢线时最大值取旧值 
235       {
236         max_front=max_old;
237         max_value=AD[max_front]; 
238       }
239       else
240         max_old=max_front;
241       
242       if(abs(max_front - max_crosstalk) < 2)  //防串道
243       {
244            max_crosstalk = max_front;
245       }
246       else 
247            max_front = max_crosstalk;
248       AD_MAX_NUM = max_front;        //传送速度控制
249       
250       /****************位置解算************************/
251       if(max_front==0 && (AD[1] <= Position_transit[0] - 1))  //已经偏离0号传感器   
252       {
253             position=0;  
254       }   
255       else if((max_front==0 && (AD[1] > Position_transit[0] + 1)) || (max_front==1 && (AD[0] - AD[2]) > 1))  //左侧位置  0-1号传感器之间
256       {    
257             position=1; 
258             AD_0_max = AD[0];  //记录下此时的3号传感器的值
259       }
260       else if((max_front==1 && (AD[2] - AD[0]) > 1) || (max_front==2 && (AD[1] > Position_transit[1] + 1)))  //右侧位置  1-2号传感器之间
261       {
262             position=2;
263             AD_2_max = AD[2];  //记录下此时的3号传感器的值
264       }
265       else if(max_front==2 && (AD[1] <= Position_transit[1] - 1))  //已经偏离3号传感器    
266       {        
267             position = 3;  
268       }  
269       //~~~~~~~~~~~~~~~~~~~~~~~ 后排位置解算~~~~~~~~~~~~~~~~~~~//
270       if(max_back == 3 && AD[4] <= Position_transit[2] - 1)
271       { 
272            if(AD[4] <= Position_transit[2] - 22)
273               position_back = 0;
274            if(AD[4] >= Position_transit[2] - 20)
275               position_back = 1;
276       }
277       else  if(max_back == 3 && AD[4] > Position_transit[2] + 1 || max_back == 4 && AD[3] > Position_transit[3] + 1)
278       {
279            position_back = 2;
280       }
281       else if(max_back == 4 && AD[3] <= Position_transit[3] - 1)
282       {
283            if(AD[3] >= Position_transit[3] - 20)
284               position_back = 3;
285            if(AD[3] <= Position_transit[3] - 22)
286               position_back = 4;
287       }
288       
289       if(abs(position - position_last) == 2)  //位置防跳变
290            position = position_last;
291       position_last = position;
292       
293      //////弯道内和导线夹角过大导致后面转向不足,此时过渡点强制增大//////////////  
294       if(position == 0 && AD_0_max < 75)
295       {
296           AD_0_max = 75 + abs_f(75 - AD_0_max);    
297       }
298       else if(position == 3 && AD_2_max < 75)
299       {
300           AD_2_max = 75 + abs_f(75 - AD_2_max); 
301       }
302       
303       /*************计算偏移量*************/
304       if(position == 0)   //左侧丢线  
305       {
306            chazhi = (int16)((AD[1] - abs_f(AD_0_max-AD[0]) - AD_0_max)*1.3)-25;
307       }
308       else if(position == 1 || position == 2) //处于中间位置
309       {
310                chazhi = (int16)(AD[2] - AD[0]);           
311       }
312       else if(position == 3)  //右侧丢线
313       {
314            chazhi = (int16)((abs_f(AD_2_max-AD[2]) + AD_2_max - AD[1] )*1.5)+30;
315       }
316       if(Stright_Flag)
317       {
318           chazhi = (int16)(((AD[2]-AD[0])-(AD[4]-AD[3])/10)*1.3);  //长直道用斜率控制舵机
319       }
320       if(UPhill_flag)  //上坡时 强制用两侧电感偏移量
321       {
322           chazhi = (int16)((AD[2] - AD[0])); 
323       }
324       
325 }

 

 三、算法解读(为了不本末倒置,不再像前面一样写太多于算法之外的东西(例如过渡点存储到了flash能掉电保存),软件滤波——关于这些我有机会再开一篇慢慢说)

路径分区核心在这:

 38    if(K2) 
 39    {
 40        LCD_Print(25,2,"Collecting"); 
 41        LCD_Print(28,4,"samples..."); 
 42    
 43        max_v[0] = max_v[1] = max_v[2] = max_v[3] = max_v[4] = 0;
 44        min_v[0] = min_v[1] = min_v[2] = min_v[3] = min_v[4] = 7;  
 45        for(i=0;i<1200;i++) 
 46        {
 47            AD_valu[0] = ad_ave(ADC1,AD9,ADC_10bit,6);  //PTC0  通道      
 48            AD_valu[1] = ad_ave(ADC1,AD8,ADC_10bit,6);  //PTC1  通道     
 49            AD_valu[2] = ad_ave(ADC1,AD15,ADC_10bit,6); //PTE25 通道     
 50            AD_valu[3] = ad_ave(ADC1,AD11,ADC_10bit,6); //PTE24 通道 
 51            AD_valu[4] = ad_ave(ADC1,AD13,ADC_10bit,6); //PTE24 通道 
 52            for(j=0;j<5;j++) 
 53            {      
 54                if(AD_valu[j] > max_v[j]) 
 55                {
 56                    max_v[j] = AD_valu[j];
 57                    if(j==0)    Position_transit_short[0] =  AD_valu[1];  //记录过渡点 电感值
 58                    if(j==2)    Position_transit_short[1] =  AD_valu[1];  
 59                    if(j==3)    Position_transit_short[2] =  AD_valu[4];  
 60                    if(j==4)    Position_transit_short[3] =  AD_valu[3];  
 61                }
 62            }
能传图片了,原来是我看错了格式。。。

没有这个图还真不好讲,了解之前最好要有一定物理认知,图中蓝线为电磁线(实际只有一条,我画了三种位置),1、电磁线与电感(图中红色)垂直有最大感应电动势,平行时最小;2、赛车采集电感值时图中两条黑线(前瞻)左右最大赛道范围摆动,即一定会得到垂直位置,就能得到最大值。
PS:K2是一个按键,即一个io口
检测到k2闭合,(配置初始低电平(下拉),K2按下,接到高电位,检测为高电平1,开始执行)
 40        LCD_Print(25,2,"Collecting"); 
 41        LCD_Print(28,4,"samples..."); 
在OLED显示,提示作用,(25,2):坐标
 45        for(i=0;i<1200;i++) 
采集执行的时间
47            AD_valu[0] = ad_ave(ADC1,AD9,ADC_10bit,6);  //PTC0  通道      
48            AD_valu[1] = ad_ave(ADC1,AD8,ADC_10bit,6);  //PTC1  通道     
49            AD_valu[2] = ad_ave(ADC1,AD15,ADC_10bit,6); //PTE25 通道     
50            AD_valu[3] = ad_ave(ADC1,AD11,ADC_10bit,6); //PTE24 通道 
51            AD_valu[4] = ad_ave(ADC1,AD13,ADC_10bit,6); //PTE24 通道 
五个电感采集数值放大后送入AD口,
ad_ave(ADC1,AD9,ADC_10bit,6) 采集6次取均值

重要部分:
 52            for(j=0;j<5;j++) 
 53            {      
 54                if(AD_valu[j] > max_v[j]) 
 55                {
 56                    max_v[j] = AD_valu[j];
 57                    if(j==0)    Position_transit_short[0] =  AD_valu[1];  //记录过渡点 电感值
 58                    if(j==2)    Position_transit_short[1] =  AD_valu[1];  
 59                    if(j==3)    Position_transit_short[2] =  AD_valu[4];  
 60                    if(j==4)    Position_transit_short[3] =  AD_valu[3];  
 61                }
 62            }
理解如下:1200次的时间内电感有时间摆动几个周期,即得到了最大值,比如在上面第一条蓝色线位置,此时得到的AD_valu[0]赋给max_v[0],注意这句if(j==0)    Position_transit_short[0] =  AD_valu[1];它把0号电感最大时1号电感的值 AD_valu[1]赋给了Position_transit_short[0];称Position_transit_short[0]为过渡点,后面同理,记下来当某个电感值最大时,另一电感的值。
下面跳到
225       for(i=0;i<3;i++)                 //找出最强的传感器
226       {  
227         if(AD[max_front]<AD[i]-2)
228           max_front=i;
229       } 
230       max_value=AD[max_front]; 
231       
232       max_back = (AD[3]>AD[4])? 3:4;  //找后排最强电感
这提出来目的是为下面服务,以免不知到为什么跳出来最强电感值(这找最大值就不用说了,都能看懂)

又是一关键部分:
250       /****************位置解算************************/
251       if(max_front==0 && (AD[1] <= Position_transit[0] - 1))  //已经偏离0号传感器   63为偏离0号传感器时1号传感器的值
252       {
253             position=0;  
254       }   
255       else if((max_front==0 && (AD[1] > Position_transit[0] + 1)) || (max_front==1 && (AD[0] - AD[2]) > 1))  //左侧位置  0-1号传感器之间
256       {    
257             position=1; 
258             AD_0_max = AD[0];  //记录下此时的3号传感器的值
259       }
260       else if((max_front==1 && (AD[2] - AD[0]) > 1) || (max_front==2 && (AD[1] > Position_transit[1] + 1)))  //右侧位置  1-2号传感器之间
261       {
262             position=2;
263             AD_2_max = AD[2];  //记录下此时的3号传感器的值
264       }
265       else if(max_front==2 && (AD[1] <= Position_transit[1] - 1))  //已经偏离3号传感器    70为偏离3号传感器时2号传感器的值
266       {        
267             position = 3;  
268       } 
position是区域,例如,0,1,2三个电感划出来了4个区域,分别为
position=0;
position=1;
position=2;
position=3;
对应图中4个区域

if(max_front==0 && (AD[1] <= Position_transit[0] - 1))
max_front==0说前排3个电感此时0号电感值最大(不是就一定在垂直位置[垂直位置是指0号电感所有值中最大的值的位置]),(AD[1] <= Position_transit[0] - 1)说1号电感的值小于过渡点的值,说明1号电感与漆包线位置距离大于过渡点时距离,漆包线位置只能在0号电感左面(最好自己拿一辆车按这个电感布局试一下,看看值的变化),记为
position=0;
else if((max_front==0 && (AD[1] > Position_transit[0] + 1)) || (max_front==1 && (AD[0] - AD[2]) > 1))
同理分析(max_front==0 && (AD[1] > Position_transit[0] + 1))

(max_front==1 && (AD[0] - AD[2]) > 1加上了这个(我自己用的时候没用这个)理解为上了个保险,这个更好理解,1号电感最大时,0号电感值又大于2号,漆包线位置只能在0~1之间。
后面同理,后排电感位置也能分出。

以上就轻松分出了位置,不同的位置对应了车子不同的姿态,再根据电感值差值,或者比值,就可以在不同位置给予不同系数(舵机打角控制 ,电机转速控制),核心是根据五个值分出位置。