恩智浦智能车分为三组：摄像头、光电、电磁，我做的是电磁车，三种车队区别在于传感器的不同，所以获得路径信息的方法也不一样，摄像头和光电识别的是赛道上的黑线（白底赛道），而电磁车则是检测埋在赛道下的通入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模块，那对于整车布局就十分麻烦，所以只好用啦（别人都在用，一起跳坑）；处理完了，接下来关门放算法。
二、核心路径算法
- /******************** (C) COPYRIGHT 2011 ********************* ********************
- * 文件名 ：Date_analyse.c
- * 描述 ：电感数据采集与分析
- *
- * 实验平台 ：野火kinetis开发板
- * 库版本 ：
- * 嵌入系统 ：
- *
- * 作者 ：o***殇のＳo
- **********************************************************************************/
- #include "include.h"
- #define NM 3
- int16 AD_valu[5],AD_V[5][NM],chazhi,chazhi_old;
- float AD[5],sensor_to_one[5];
- float Slope_AD_1; // 用于坡道检测
- int16 max_v[5],min_v[5]; //电感标定 采集值
- int16 Position_transit[4]; //记录过渡点归一化的值
- int16 AD_sum[5];
- int16 AD_MAX_NUM; //
- int16 position = 2,position_back = 1;
- float max_value,AD_0_max,AD_1_max,AD_2_max,AD_3_max;
- /*************************************************************************
- * 函数名称 SC_black_Init
- * 功能说明： 最大值采样
- * 参数说明：
- * 函数返回： 无
- * 修改时间：
- * 备 注：
- *************************************************************************/
- void SC_black_Init(void)
- {
- uint16 i,j;
- int16 Position_transit_short[4];
- float sensor_1,sensor_2,sensor_3,sensor_4;
- if(K2)
- {
- LCD_Print(25,2,"Collecting");
- LCD_Print(28,4,"samples...");
- max_v[0] = max_v[1] = max_v[2] = max_v[3] = max_v[4] = 0;
- min_v[0] = min_v[1] = min_v[2] = min_v[3] = min_v[4] = 7;
- for(i=0;i<1200;i++)
- {
- AD_valu[0] = ad_ave(ADC1,AD9,ADC_10bit,6); //PTC0 通道
- AD_valu[1] = ad_ave(ADC1,AD8,ADC_10bit,6); //PTC1 通道
- AD_valu[2] = ad_ave(ADC1,AD15,ADC_10bit,6); //PTE25 通道
- AD_valu[3] = ad_ave(ADC1,AD11,ADC_10bit,6); //PTE24 通道
- AD_valu[4] = ad_ave(ADC1,AD13,ADC_10bit,6); //PTE24 通道
- for(j=0;j<5;j++)
- {
- if(AD_valu[j] > max_v[j])
- {
- max_v[j] = AD_valu[j];
- if(j==0) Position_transit_short[0] = AD_valu[1]; //记录过渡点 电感值
- if(j==2) Position_transit_short[1] = AD_valu[1];
- if(j==3) Position_transit_short[2] = AD_valu[4];
- if(j==4) Position_transit_short[3] = AD_valu[3];
- }
- }
- delayms(1); //延时
- }
- /*************** 记录的过渡点归一化 ******************/
- sensor_1 = (float)(Position_transit_short[0] - min_v[1])/(float)(max_v[1] - min_v[1]);
- if(sensor_1 <= 0.0) sensor_1 = 0.001;
- if(sensor_1 >= 1.0) sensor_1 = 1.0;
- sensor_2 = (float)(Position_transit_short[1] - min_v[1])/(float)(max_v[1] - min_v[1]);
- if(sensor_2 <= 0.0) sensor_2 = 0.001;
- if(sensor_2 >= 1.0) sensor_2 = 1.0;
- sensor_3 = (float)(Position_transit_short[2] - min_v[4])/(float)(max_v[4] - min_v[4]);
- if(sensor_3 <= 0.0) sensor_3 = 0.001;
- if(sensor_3 >= 1.0) sensor_3 = 1.0;
- sensor_4 = (float)(Position_transit_short[3] - min_v[3])/(float)(max_v[3] - min_v[3]);
- if(sensor_4 <= 0.0) sensor_4 = 0.001;
- if(sensor_4 >= 1.0) sensor_4 = 1.0;
- Position_transit[0] = (int16)(100 * sensor_1);
- Position_transit[1] = (int16)(100 * sensor_2);
- Position_transit[2] = (int16)(100 * sensor_3);
- Position_transit[3] = (int16)(100 * sensor_4);
- flash_erase_sector(SECTOR_ADM); //擦除254扇区
- for(i=0; i<5; i++) //电感标定的最大值写入扇区
- {
- flash_write(SECTOR_ADM,i*4,max_v);
- }
- for(i=0;i<4;i++) //过渡点归一化值写入扇区
- {
- flash_write(SECTOR_ADM,20+i*4,Position_transit);
- }
- }
- else
- {
- for(i=0;i<3;i++)
- {
- for(j=0;j<5;j++) //读取五个电感的采样标定的最大值
- {
- max_v[j] = flash_read(SECTOR_ADM,j*4,int16);
- }
- for(j=0;j<4;j++) //读取过渡点
- {
- Position_transit[j] = flash_read(SECTOR_ADM,20+j*4,int16);
- }
- LCD_Print(29,2,"Reading");
- LCD_Print(28,4,"samples...");
- delayms(10);
- }
- }
- LCD_CLS();
- Beer_ON;
- delayms(25);
- Beer_OFF;
- }
- /*************************************************************************
- * 函数名称 Read_ADC
- * 功能说明： AD采集
- * 参数说明：
- * 函数返回： 无
- * 修改时间：
- * 备 注：
- *************************************************************************/
- void Read_ADC(void)
- {
- int16 i,j,k,temp;
- int16 ad_valu[5][5],ad_valu1[5],ad_sum[5];
- for(i=0;i<5;i++)
- {
- ad_valu[0]=ad_ave(ADC1,AD9,ADC_10bit,7); // ADC0 通道
- ad_valu[1]=ad_ave(ADC1,AD8,ADC_10bit,7); // ADC0 通道
- ad_valu[2]=ad_ave(ADC1,AD15,ADC_10bit,7); // ADC0 通道
- ad_valu[3]=ad_ave(ADC1,AD11,ADC_10bit,7); // ADC0 通道
- ad_valu[4]=ad_ave(ADC1,AD13,ADC_10bit,7); // ADC0 通道
- }
- //////////////////////冒泡排序///////////////////////////////////
- for(i=0;i<5;i++) //5个电感
- {
- for(j=0;j<4;j++) //五个数据排序
- {
- for(k=0;k<4-j;k++)
- {
- if(ad_valu[k] > ad_valu[k+1]) //前面的比后面的大 则进行交换
- {
- temp = ad_valu[k+1];
- ad_valu[k+1] = ad_valu[k];
- ad_valu[k] = temp;
- }
- }
- }
- }
- for(i=0;i<5;i++) //求中间三项的和
- {
- ad_sum = ad_valu[1] + ad_valu[2] + ad_valu[3];
- ad_valu1 = ad_sum / 3;
- }
- ////////////////////////滑动平均滤波/////////////////////////////
- for(i = 0;i < NM-1;i ++)
- {
- AD_V[0] = AD_V[0][i + 1];
- AD_V[1] = AD_V[1][i + 1];
- AD_V[2] = AD_V[2][i + 1];
- AD_V[3] = AD_V[3][i + 1];
- AD_V[4] = AD_V[4][i + 1];
- }
- for(i=0;i<5;i++)
- {
- AD_V[NM-1] = ad_valu1;
- }
- for(i = 0;i < NM;i ++)
- {
- AD_sum[0] += AD_V[0];
- AD_sum[1] += AD_V[1];
- AD_sum[2] += AD_V[2];
- AD_sum[3] += AD_V[3];
- AD_sum[4] += AD_V[4];
- }
- for(i=0;i<5;i++) //求平均
- {
- AD_valu = AD_sum / NM;
- AD_sum = 0;
- }
- }
- /*************************************************************************
- * 函数名称 Date_analyse
- * 功能说明： 数据分析
- * 参数说明：
- * 函数返回： 无
- * 修改时间：
- * 备 注：
- *************************************************************************/
- void Date_analyse()
- {
- int16 i,max_front=0,max_back;
- static int16 max_old = 1,max_crosstalk = 1;
- static int16 position_last = 2;
- float sensor_1;
- Read_ADC();
- /*********************归一化处理********************/
- for(i=0;i<5;i++)
- {
- sensor_to_one = (float)(AD_valu - min_v)/(float)(max_v - min_v);
- if(sensor_to_one<=0.0) sensor_to_one=0.001;
- if(sensor_to_one>1.0) sensor_to_one=1.0;
- AD = 100 * sensor_to_one; //AD为归一化后的值 范围为0-100
- }
- /*******1号电感特殊归一化，用于坡道检测********/
- sensor_1 = (float)(AD_valu[1] - min_v[1])/(float)(max_v[1] - min_v[1]);
- if(sensor_1 <= 0.0) sensor_1 = 0.001;
- Slope_AD_1 = 100 * sensor_1;
- ////////////////////////////////////////////////////////////////////
- for(i=0;i<3;i++) //找出最强的传感器
- {
- if(AD[max_front]-2)
- max_front=i;
- }
- max_value=AD[max_front];
- max_back = (AD[3]>AD[4])? 3:4; //找后排最强电感
- if(max_value < 43) //丢线时最大值取旧值
- {
- max_front=max_old;
- max_value=AD[max_front];
- }
- else
- max_old=max_front;
- if(abs(max_front - max_crosstalk) < 2) //防串道
- {
- max_crosstalk = max_front;
- }
- else
- max_front = max_crosstalk;
- AD_MAX_NUM = max_front; //传送速度控制
- /****************位置解算************************/
- if(max_front==0 && (AD[1] <= Position_transit[0] - 1)) //已经偏离0号传感器 63为偏离0号传感器时1号传感器的值
- {
- position=0;
- }
- else if((max_front==0 && (AD[1] > Position_transit[0] + 1)) || (max_front==1 && (AD[0] - AD[2]) > 1)) //左侧位置 0-1号传感器之间
- {
- position=1;
- AD_0_max = AD[0]; //记录下此时的3号传感器的值
- }
- else if((max_front==1 && (AD[2] - AD[0]) > 1) || (max_front==2 && (AD[1] > Position_transit[1] + 1))) //右侧位置 1-2号传感器之间
- {
- position=2;
- AD_2_max = AD[2]; //记录下此时的3号传感器的值
- }
- else if(max_front==2 && (AD[1] <= Position_transit[1] - 1)) //已经偏离3号传感器 70为偏离3号传感器时2号传感器的值
- {
- position = 3;
- }
- //~~~~~~~~~~~~~~~~~~~~~~~ 后排位置解算~~~~~~~~~~~~~~~~~~~//
- if(max_back == 3 && AD[4] <= Position_transit[2] - 1)
- {
- if(AD[4] <= Position_transit[2] - 22)
- position_back = 0;
- if(AD[4] >= Position_transit[2] - 20)
- position_back = 1;
- }
- else if(max_back == 3 && AD[4] > Position_transit[2] + 1 || max_back == 4 && AD[3] > Position_transit[3] + 1)
- {
- position_back = 2;
- }
- else if(max_back == 4 && AD[3] <= Position_transit[3] - 1)
- {
- if(AD[3] >= Position_transit[3] - 20)
- position_back = 3;
- if(AD[3] <= Position_transit[3] - 22)
- position_back = 4;
- }
- if(abs(position - position_last) == 2) //位置防跳变
- position = position_last;
- position_last = position;
- //////弯道内和导线夹角过大导致后面转向不足，此时过渡点强制增大//////////////
- if(position == 0 && AD_0_max < 75)
- {
- AD_0_max = 75 + abs_f(75 - AD_0_max);
- }
- else if(position == 3 && AD_2_max < 75)
- {
- AD_2_max = 75 + abs_f(75 - AD_2_max);
- }
- /*************计算偏移量*************/
- if(position == 0) //左侧丢线
- {
- chazhi = (int16)((AD[1] - abs_f(AD_0_max-AD[0]) - AD_0_max)*1.3)-25;
- }
- else if(position == 1 || position == 2) //处于中间位置
- {
- chazhi = (int16)(AD[2] - AD[0]);
- }
- else if(position == 3) //右侧丢线
- {
- chazhi = (int16)((abs_f(AD_2_max-AD[2]) + AD_2_max - AD[1] )*1.5)+30;
- }
- if(Stright_Flag)
- {
- chazhi = (int16)(((AD[2]-AD[0])-(AD[4]-AD[3])/10)*1.3); //长直道用斜率控制舵机
- }
- if(UPhill_flag) //上坡时 强制用两侧电感偏移量
- {
- chazhi = (int16)((AD[2] - AD[0]));
- }
- }
[color=rgb(51, 102, 153) !important]复制代码
三、算法解读
- 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 }
[color=rgb(51, 102, 153) !important]复制代码
没有这个图还真不好讲，了解之前最好要有一定物理认知，图中蓝线为电磁线（实际只有一条，我画了三种位置），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 }
[color=rgb(51, 102, 153) !important]复制代码
理解如下：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]-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 }
[color=rgb(51, 102, 153) !important]复制代码
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号电感值最大（不是就一定在垂直位置），(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之间。
后面同理，后排电感位置也能分出。
以上就轻松分出了位置，不同的位置对应了车子不同的姿态，再根据电感值差值，或者比值，就可以在不同位置给予不同系数（舵机打角控制 ，电机转速控制），核心是根据五个值分出位置。 |
2
/6 
工商网监
湘ICP备2023018690号
7990
淘帖
1438
发表评论
赞
回复
