这篇文章介绍如何让车真正跑起来。即按照规定路径循迹。
为了让小车能够在赛道上行驶,至少会使用左、右两个电感,
通过ADC采集经过运放处理的感应电动势值来计算偏差,
方法是将差比和计算出来的偏差以一定的比例关联到舵机的pwm占空比来控制舵机打角,进而控制小车进行转向
目录
一、电磁循迹原理
先简单讲一下电磁循迹的原理。
电磁场地是赛道中央铺设电磁线,电磁线的两头接信号发生器。
信号发生器就是产生50HZ的正弦信号,
“电生磁”,电磁线通有正弦变化的电流,会在周围产生正弦的磁场,
“磁生电”,工字电感就是由N圈线圈构成,变化的磁场+运动切割磁感线,会产生感应电流,
经过运放和AD转换得到我们的电磁信号值。
电感距离磁场越近,产生的感应电动势越大,距离越远,产生的感应电动势越小。
电感的布局,影响着 运动切割磁感线产生的电流,对循迹与元素识别起着重大影响
直观地来说,将车静置在赛道中央,同一水平高度上,与电磁线方向垂直放置的电感,获取的值大。
把车放到赛道上,各种姿势各种方向都摆一遍,就能很清楚知道电感方向和铜线方向的关系。
出于习惯,我会把“电感电容对→运放→ADC”这个过程获取到的值成为电感值或者电感电容对的值。
恩智浦智能车四轮组-- 1.电磁检测方案、运放选型_Doee hc的博客-CSDN博客
二、排布方案浅析
电感排布方案是决定电磁智能车竞赛成绩的重要因素之一。接下来,我们将探讨如何确定一个优秀的电感排布方案,通过不同的布局方式来提高电磁车在赛道上的性能。
1.二水平电感循迹
这里引用逐飞的讲解,原文地址:四轮电磁组之逐飞演示车模浅析
常用的电磁寻迹算法采用差比和算法,在讲解差比和算法之前我们先看看下图:
首先要了解车模位于赛道中的不同位置时所采集的电感数据。参加比赛的同学在学习智能车如何制作前都会先阅读规则,规则中有说明,电磁引导是通过在赛道中心铺设漆包线并通以20khz的交变信号。我们通过中学物理知识可以得知,导线通以交变电流之后会产生交变磁场,电感线圈在交变磁场中会产生交变电压,电感距离导线越近电感产生的电压峰峰值则越大,通过对电感感应出来的电压进行放大并整流可以得到一个直流信号,直流信号电压越高距离导线则越近。需要注意的是电感摆放的位置需要让磁感线能够穿过电感的线圈,这样才能正确的感应到磁场大小。接下来我们根据距离导线越近数值越大的理论来合理的假设电感的数据,然后建立一个表格来分析下数据并使用差比和公式计算结果,差比和的公式为(a-b)/(a+b)。
车模位置
电感1数值
电感2数值
差比和计算的值
图1
50
50
0
图2
25
75
-0.5
图3
75
25
0.5
从计算出来的结果我们看出来,当车模放置在赛道中间时计算出来的数值为0,当车模放置在赛道的左侧时计算出来的数值为-0.5,当车模放置在赛道的右侧时计算出来的数值为+0.5。那么我们可以通过判断计算出来的数值符号来判断车模在赛道的左边还是右边。那么差比和计算出来的数值又表示什么呢?我们可以在假设两组数据来计算一下,将图2与图3中车模的位置都往赛道中心线靠近一点,我们知道电感距离漆包线越近所感应到的电压也就越大。因此图2中车模移动位置之后电感1的数值将会变大,电感2的数值将会变小,同理图3中电感1的数值将会变小,电感2的数值将会变大。通过计算后:
车模位置
电感1数值
电感2数值
差比和计算的值
图2往赛道中心移动一点
35
65
-0.3
图3往赛道中心移动一点
65
35
0.3
我们会发现,车模距离赛道中心线的距离变近之后差比和计算出来的绝对值也就变小了。因此数值大小表示偏离赛道的程度,在一定范围内车模偏离赛道越远计算出来的值越大。得到了车模偏离赛道的程序之后我们就可以用此数据控制舵机,来使得车模一直沿着赛道中心线前进了。
注:
1. ADC分辨率与数据类型:
- 对于电磁寻迹,通常8位ADC分辨率足够。在初始化ADC时,选择了8位的分辨率,因此
ad_left
和ad_right
被定义为8位无符号类型。 - 由于计算结果可能有正有负,因此
position
定义为有符号16位类型。在计算差时,需进行强制转换或者直接定义为有符号类型。
2. 边界保护:
- 为防止车模在运动中位置偏离赛道,采用和值的边界保护。如果和值小于设定值,则保留上次结果,不进行计算,确保结果合理。
3. 差比和的计算公式:
- 由于除数不能为0,和值固定加一。在没有引入边界保护的情况下,和值不可能为0。
- 两种放大方法:放大100倍和放大128倍,选择环移方式放大128倍,以提高运算速度。
4. 放大运算注意事项:
- 放大运算时,先放大再除,避免出现整数运算时的错误结果。
2.三水平电感循迹
这里引用逐飞的讲解,原文地址:专科电磁组之逐飞演示车模浅析
三电感的寻迹算法,这个算法有一些优势也有一些劣势,因此它并不是最完美的,还需要大家多多思考如何完善它的一些缺点。
三电感算法顾名思义也就是用三个电感进行寻迹,使用左右的电感来判断小车应该往哪个方向转弯,使用中间电感来判断应该转多大的弯。
首先我们需要知道一个基本的概念,当电感距离电磁线越近则感应到的电压也越大,反之则越小。我们来分别将小车放在赛道的中间、左边、右边。
电感
小车位置
电感1
电感3
电感2
中间
50
75
50
左边
25
50
75
右边
75
50
25
小车只要不在中间,电感1与电感2就不会相等,我们就可以通过这两个电感的差值,来判断小车应该往哪个方向转向。转角的大小我们可以采用电感3的最大值减去电感3实时的值,差值遇到转角也就越大。
三电感算法的优点在于:
1.小车在直道不容易左右晃动,原因在于电感3并不是只在赛道中间才是最大值,而是在中间附近小范围都是最大值,这样小车在直道上就不会频繁的打角。
2.由于转角的大小是采用电感3的最大值减去实时值,因为当电感逐渐远离赛道的时候,计算出来的转角只会越来越大,不存在差比和算法转角会逐渐变小的问题。
3.由于转角值计算是采用的一个电感,因为就不存在差比和中电感对称性的问题。
三电感算法的缺点:
1.虽然小车直道不容易晃,但是同时也不能让小车锁定着中线前行。
2.前瞻特别长的时候,可能需要对方向计算进行一定的保护,避免左右电感都为0的时候计算出不正确的方向。
3. 八字形电感排布方案
八字形电感排布方案同样能够在直道行驶时良好地采集数据。通过计算两端电感的数值差异,可以判断小车与赛道中心线的偏离情况,并做出相应的方向控制。在弯道情况下,与赛道电磁线呈较好的角度,提高了对赛道信息的精确度。
然而,八字形电感排布在直道行驶时无法充分发挥优势。
4. 双T形电感排布方案
双T形电感排布方案综合了双水平和八字形的优势,能够在直道和弯道行驶时都取得良好的效果。通过采集每个电感的数值,并进行合理处理,我们可以准确解算出电磁车在赛道上的位置,实现对电磁车的精准控制。
除了上图所示的排布方式,也可以采用其他形式的排布,原理是一样的。通过选择合适的电感排布方案,可以使电磁车在赛道上获取更准确、更充分的信息,提高竞赛表现。这为电磁智能车的设计和调优提供了有益的参考。
我们当时对电容电感对的排布也处于摸索阶段,于是设计出下面的排布方案。
三、差比和算法
智能车电感差比和差加权算法研究_卓晴的博客-CSDN博客_差比和算法
差比和是 电磁组循迹比较基础的算法,
差比和:
- 读取左右两个电磁感应模块的电磁信号值;
- 对左右两个电磁感应模块的电磁信号值进行差分处理,得到一个差值;
- 根据差值的正负来判断小车是否偏离了中线;
- 根据差值的大小来调整小车的转向角度,使其重新回到中线上。
电磁组比较简单,经过这么多年的发展,应该还有其他的加权算法,就需要你们探索和借鉴。
四、简单巡线
1.采集电感值
通过adc采集函数,获取到电感值。
在进入新的场地的时候,需要调节运放,使电感值在一定的范围。
放大倍数太低,不利于元素识别,
放大倍数太高,会在车贴近地面/环岛时,电感值爆满,不能正常循迹
2.归一化与滤波
就是用第一步采集的最大值max(左右对称的电感用同一个),用adc采集的值value。进行 value/max*100,将值变到0~100的区间内。这样做的好处是,1、可以方便自己对数据的感知,在普通元素和特殊元素间;2、在赛道更换后,测新的赛道的最大值,改变max的值即可,有较强的适应性。3、方便数据处理。
3.加权算法
利用算法处理得到的电感值,得到偏差值
4.由偏差值和其他信息,
控制电机转速与舵机打角。
【智能车学习】电磁循迹中的基本控制算法_Carry qinginger的博客-CSDN博客_电磁循迹
直接调节电感差比和差加权算法可能会因无法直观的了解各参数而走弯路,建议先调出一套相对稳定的电感差比和加权方案,然后将其改写为电感差比和差加权算法。
使用差比和差加权算法时,小车在弯道道型出现小幅度过调,此时轻微降低比例系数即可完全拟合。
下面是我当时用的电感值处理方案。
#include "inductance.h"
#include <math.h>
uint16 LNow[10];
float LNor[10];
uint16 LNowinit[10] = {1300,1200,1500,1200,1300,1300,1500,1300,3700,3700};
uint16 LNORMAX[10] = {2980,2000,2000,2980,3350,1500,1500,3350,3700,3700};
uint16 ELC[ELC_GROUP][ELC_NUM];
//偏差
int32 ELC_Center_14 = 0;
int32 ELC_Center_23 = 0;
int32 ELC_Center_58 = 0;
int32 ELC_Center0 = 0;
int32 ELC_Center1 = 0;
int32 ELC_Center = 0;
int32 ELC_CenterRepair = 0;
int32 ELC_Center_Last = 0;
//电感和
uint16 ELC_Sum_14 = 0;
uint16 ELC_Sum_23 = 0;
uint16 ELC_Sum_58 = 0;
uint16 ELC_Sum_1234 = 0;
uint16 ElC_Sum_146=0;
//电感差
int16 ELC_Err_14 = 0;
int16 ELC_Err_23 = 0;
int16 ELC_Err_58 = 0;
//权重系数
float K1=0.8f;
float K2=0;
float K3=0.1f;
float K4=0.1f;
//电感保护
uint8 ELC_PROTECT_FLAG=0;
//电感处理
void Inductor_deal(void)
{
uint16 L_sum[ELC_NUM] = {0,0,0,0,0,0,0,0,0,0,};
int8 i = 0,k = 0;
uint16 ELC_max[ELC_NUM] = {4095,4095,4095,4095,4095,4095,4095,4095,4095,4095};
uint16 ELC_min[ELC_NUM] = {0,0,0,0,0,0,0,0,0,0,};
//读取ADC
for(i = 0;i < ELC_GROUP ;i ++)
{
ELC[i][0] = adc_once(ADC1,ADC_12BIT);
ELC[i][1] = adc_once(ADC2,ADC_12BIT);
ELC[i][2] = adc_once(ADC3,ADC_12BIT);
ELC[i][3] = adc_once(ADC4,ADC_12BIT);
ELC[i][4] = adc_once(ADC5,ADC_12BIT);
ELC[i][5] = adc_once(ADC6,ADC_12BIT);
ELC[i][6] = adc_once(ADC7,ADC_12BIT);
ELC[i][7] = adc_once(ADC8,ADC_12BIT);
ELC[i][8] = adc_once(ADC9,ADC_12BIT);
ELC[i][9] = adc_once(ADC10,ADC_12BIT);
}
//挑出最大最小值
for(k = 0; k < ELC_NUM ; k++)
{
for(i = 0; i < ELC_GROUP; i++)
{
if(ELC[i][k] >= ELC_min[k])
{
ELC_min[k]=ELC[i][k];
}
if(ELC[i][k] <= ELC_max[k])
{
ELC_max[k]=ELC[i][k];
}
L_sum[k] += ELC[i][k];
}
}
//第一次归一化
if(Fork.state == Fork_begin)
{
for(i = 0;i < ELC_NUM;i ++)
{
LNow[i] = ((L_sum[i]-ELC_max[i]-ELC_min[i])/ (ELC_GROUP-2));
LNow[i] = LNow[i]+1;
}
}
else
{
for(i = 0;i < ELC_NUM;i ++)
{
LNow[i] = 2.2*(L_sum[i]-ELC_max[i]-ELC_min[i])/ (ELC_GROUP-2);
LNow[i] = LNow[i]+1;
}
}
//第二次归一化
for(i = 0;i < ELC_NUM;i++)
{
LNor[i] = LNow[i];
LNor[i] = LNor[i]*100/LNORMAX[i];
}
//电磁保护
if(max_ab(LNow[0],LNow[3])<= 20&&max_ab(LNow[1],LNow[2])<= 20&&max_ab(LNow[4],LNow[7])<= 500&&LNow[6]<40)
ELC_PROTECT_FLAG = 'T';
else
ELC_PROTECT_FLAG = 'F';
//电感值差
ELC_Err_14 = LNor[3] - LNor[0];
ELC_Err_23 = LNor[2] - LNor[1];
ELC_Err_58 = LNor[7] - LNor[4];
//电感值和
ELC_Sum_14 = LNor[0] + LNor[3];
ELC_Sum_58 = LNor[4] + LNor[7];
ELC_Sum_23 = LNor[1] + LNor[2];
ELC_Sum_1234= ELC_Sum_14 + ELC_Sum_23;
//差比和处理
// ELC_Center_14=(int16)((float)1000*(sqrt(LNor[3])-sqrt(LNor[0]))/(LNor[3]+ LNor[0]));
// ELC_Center_23=(int16)((float)1000*(sqrt(LNor[2])-sqrt(LNor[1]))/(LNor[2]+ LNor[1]));
// ELC_Center_58=(int16)((float)1000*(sqrt(LNor[7])-sqrt(LNor[4]))/(LNor[7]+ LNor[4]));
// ELC_Center_14=(int16)((float)1400*(LNor[3]+LNor[2]-(LNor[0]+LNor[1]))/(LNor[3]+LNor[5]+ LNor[0]+LNor[1]+ LNor[2]));
// ELC_Center_23=(int16)((float)1000*(sqrt(LNor[2])-sqrt(LNor[1]))/(LNor[2]+ LNor[1]));
// ELC_Center_58=(int16)((float)1400*(sqrt(LNor[7])-sqrt(LNor[4]))/(LNor[7]+LNor[6]+LNor[4]));
// ELC_Center_23=(int16)((float)100*(LNor[2]-LNor[1])/(LNor[2]+ LNor[1]));
// ELC_Center_14=(int32)((float)900*(LNor[3]-LNor[0])/(LNor[3]*LNor[0]));
// ELC_Center_23=(int32)((float)700*(LNor[2]-LNor[1])/(LNor[2]*LNor[1]));
// ELC_Center_58=(int32)((float)800*(LNor[7]-LNor[4])/(LNor[7]*LNor[4]));
ELC_Center_14=(int32)((float)2000*(sqrt(LNor[3])-sqrt(LNor[0]))/(LNor[3]+1.5*LNor[5]+ LNor[0]));
ELC_Center_23=(int32)((float)1500*(sqrt(LNor[2])-sqrt(LNor[1]))/(LNor[2]+ LNor[1]));
ELC_Center_58=(int32)((float)1800*(sqrt(LNor[7])-sqrt(LNor[4]))/(LNor[7]+LNor[6]+LNor[4]));
//系数修正
K2=(float)myabs(8.5f*(limit_ab(ELC_Center_14,-100.0f,100.0f)/1000.0f));
K1=(float)(1.30f-K2);
// ELC_Center1=(int32)((float)120*(0.4*(LNor[3]-LNor[0])+1.2*(LNor[2]-LNor[1]))/(0.4*(LNor[3]+ LNor[0])+LNor[5]+1.0f*myabs(LNor[2]-LNor[1])));
// ELC_Center1=(int32)((float)1650*(K1*(sqrt(LNor[3])-sqrt(LNor[0]))+ K2*(sqrt(LNor[2])-sqrt(LNor[1])))/(0.4*(LNor[3]+ LNor[0])+1.5*LNor[5]+0.8f*myabs(LNor[2]-LNor[1])));
// ELC_Center1=(int32)((float)1650*(0.4*(sqrt(LNor[3])-sqrt(LNor[0]))+(sqrt(LNor[2])-sqrt(LNor[1])))/(0.4*(LNor[3]+ LNor[0])+1.5*LNor[5]+0.8f*myabs(LNor[2]-LNor[1])));
// ELC_Center0= (int32)((float)1650*(K1*(sqrt(LNor[3])-sqrt(LNor[0]))+(K2+0.2f)*(sqrt(LNor[2])-sqrt(LNor[1])))/(K1*(LNor[3]+ LNor[0])+1.5*LNor[5]+(K2+0.2)*myabs(LNor[2]-LNor[1])));
// ELC_Center0= (int32)((float)1650*(K1*(sqrt(LNor[3])-sqrt(LNor[0]))+(K2+0.2f)*(sqrt(LNor[2])-sqrt(LNor[1])))/(K1*(LNor[3]+ LNor[0])+1.5*LNor[5]+(K2+0.2)*myabs(LNor[2]-LNor[1])));
// ELC_Center1= (int32)((float)2300*(K1*(sqrt(LNor[3])-sqrt(LNor[0]))+(K2+0.2f)*(sqrt(LNor[2])-sqrt(LNor[1])))/(K1*sqrt((LNor[3]*LNor[0]))+1.5*LNor[5]+(K2+0.2)*(LNor[1]+LNor[2])));
ELC_Center0= (int32)((float)2200*(K1*(sqrt(LNor[3])-sqrt(LNor[0]))+(K2+0.2f)*(sqrt(LNor[2])-sqrt(LNor[1])))/(K1*sqrt((LNor[3]*LNor[0]))+1.5*LNor[5]+(K2+0.2)*(LNor[1]+LNor[2])));
K3=(float)myabs(5.5f*(limit_ab(ELC_Center0,-100.0f,100.0f)/1000.0f));
K4=1.0-K3;
ELC_Center1= K4*ELC_Center0+K3*ELC_Center_23;
// Elc_boundary_deal();
// ELC_Center1=ELC_Center0;
/*特殊元素,处理*/
if(Cross.FindFlag == 'T')
{
if(Cross.state==Crossin)
{
ELC_Center1= K4*ELC_Center0+(K3+0.2)*ELC_Center_23;
}
else if(Cross.state==Crosson)
{
ELC_Center1= K4*ELC_Center0+0.4*ELC_Center_58;
}
}
if(Fork.Dir == 'L'&&Fork.state == Fork_middle){ELC_Center1=0.6*ELC_Center_58+0.4*ELC_Center_14;}
if(max_ab(LNow[0],LNow[3])>=150&&max_ab(LNow[1],LNow[2])<80&&max_ab(LNow[4],LNow[7])>=150)
{
if(ELC_Center_14*ELC_Center_58>=0&&ELC_Center_23*ELC_Center_58>=0 &&ELC_Center_14*ELC_Center_23>=0 ){//都同号
if(myabs(ELC_Center_14)>myabs(ELC_Center_23))
ELC_Center1=(int16)((float)(1.2*ELC_Center_14)+(float)(0.3*ELC_Center_23));
else
ELC_Center1=(int16)((float)(1.2*ELC_Center_23)+(float)(0.3*ELC_Center_14));}
else if(ELC_Center_14*ELC_Center_58>=0&&ELC_Center_23*ELC_Center_58<0 &&ELC_Center_14*ELC_Center_23<=0){//23异, 14,58同号
if(myabs(ELC_Center_14)>myabs(ELC_Center_58))
ELC_Center1=(int16)((float)(1.2*ELC_Center_14)+(float)(0.3*ELC_Center_58));
else
ELC_Center1=(int16)((float)(1.2*ELC_Center_58)+(float)(0.3*ELC_Center_14));}
else if(ELC_Center_14*ELC_Center_58<0&&ELC_Center_23*ELC_Center_58>=0 &&ELC_Center_14*ELC_Center_23<=0){//14异, 23,58同号
if(myabs(ELC_Center_23)>myabs(ELC_Center_58))
ELC_Center1=(int16)((float)(1.2*ELC_Center_23)+(float)(0.3*ELC_Center_58));
else
ELC_Center1=(int16)((float)(1.2*ELC_Center_58)+(float)(0.3*ELC_Center_23));}
else
{
if(myabs(ELC_Center_14)>myabs(ELC_Center_23))
ELC_Center1=(int16)((float)(1.2*ELC_Center_14)+(float)(0.3*ELC_Center_23));
else
ELC_Center1=(int16)((float)(1.2*ELC_Center_23)+(float)(0.3*ELC_Center_14));
}
}
else if(max_ab(LNow[0],LNow[3])<150&&max_ab(LNow[1],LNow[2])>=80&&max_ab(LNow[4],LNow[7])>=150)
{
if(ELC_Center_14*ELC_Center_58>=0&&ELC_Center_23*ELC_Center_58>=0 &&ELC_Center_14*ELC_Center_23>=0 ){//都同号
if(myabs(ELC_Center_23)>myabs(ELC_Center_14))
ELC_Center1=(int16)((float)(1.2*ELC_Center_23)+(float)(0.3*ELC_Center_14));
else
ELC_Center1=(int16)((float)(1.2*ELC_Center_14)+(float)(0.3*ELC_Center_23));}
else if(ELC_Center_14*ELC_Center_58>=0&&ELC_Center_23*ELC_Center_58<0 &&ELC_Center_14*ELC_Center_23<=0){//23异, 14,58同号
if(myabs(ELC_Center_14)>myabs(ELC_Center_58))
ELC_Center1=(int16)((float)(1.2*ELC_Center_14)+(float)(0.3*ELC_Center_58));
else
ELC_Center1=(int16)((float)(1.2*ELC_Center_58)+(float)(0.3*ELC_Center_14));}
else if(ELC_Center_14*ELC_Center_58<0&&ELC_Center_23*ELC_Center_58>=0 &&ELC_Center_14*ELC_Center_23<=0){//14异, 23,58同号
if(myabs(ELC_Center_23)>myabs(ELC_Center_58))
ELC_Center1=(int16)((float)(1.2*ELC_Center_23)+(float)(0.3*ELC_Center_58));
else
ELC_Center1=(int16)((float)(1.2*ELC_Center_58)+(float)(0.3*ELC_Center_23));}
else
{
if(myabs(ELC_Center_14)>myabs(ELC_Center_23))
ELC_Center1=(int16)((float)(1.2*ELC_Center_14)+(float)(0.3*ELC_Center_23));
else
ELC_Center1=(int16)((float)(1.2*ELC_Center_23)+(float)(0.3*ELC_Center_14));
}
}
else if(max_ab(LNow[0],LNow[3])<150&&max_ab(LNow[1],LNow[2])<80&&max_ab(LNow[4],LNow[7])>=150)
{
if(ELC_Center_14*ELC_Center_58>=0&&ELC_Center_23*ELC_Center_58>=0 &&ELC_Center_14*ELC_Center_23>=0 ){//都同号
ELC_Center1=(int16)((float)(2.0*ELC_Center_58));}
else
ELC_Center1 = ELC_Center_Last;
// if(myabs(ELC_Center_23)>myabs(ELC_Center_14))
// ELC_Center1=(int16)((float)(1.2*ELC_Center_23)+(float)(0.3*ELC_Center_14));
// else
// ELC_Center1=(int16)((float)(1.2*ELC_Center_14)+(float)(0.3*ELC_Center_23));}
// else if(ELC_Center_14*ELC_Center_58>=0&&ELC_Center_23*ELC_Center_58<0 &&ELC_Center_14*ELC_Center_23<=0){//23异, 14,58同号
// if(myabs(ELC_Center_14)>myabs(ELC_Center_58))
// ELC_Center1=(int16)((float)(1.2*ELC_Center_14)+(float)(0.3*ELC_Center_58));
// else
// ELC_Center1=(int16)((float)(1.2*ELC_Center_58)+(float)(0.3*ELC_Center_14));}
// else if(ELC_Center_14*ELC_Center_58<0&&ELC_Center_23*ELC_Center_58>=0 &&ELC_Center_14*ELC_Center_23<=0){//14异, 23,58同号
// if(myabs(ELC_Center_23)>myabs(ELC_Center_58))
// ELC_Center1=(int16)((float)(1.2*ELC_Center_23)+(float)(0.3*ELC_Center_58));
// else
// ELC_Center1=(int16)((float)(1.2*ELC_Center_58)+(float)(0.3*ELC_Center_23));}
// else
// {
// if(myabs(ELC_Center_14)>myabs(ELC_Center_23))
// ELC_Center1=(int16)((float)(1.2*ELC_Center_14)+(float)(0.3*ELC_Center_23));
// else
// ELC_Center1=(int16)((float)(1.2*ELC_Center_23)+(float)(0.3*ELC_Center_14));
// }
}
else if(max_ab(LNow[0],LNow[3])<150&&max_ab(LNow[1],LNow[2])<80&&max_ab(LNow[4],LNow[7])<150)
{
ELC_Center1 = ELC_Center_Last;
}
ELC_Center=KalmanFilter(&KFP_ELCCenter,ELC_Center1);
ELC_Center_Last = ELC_Center;
}
void Elc_boundary_deal(void)
{
//14正常23正常58正常
if(max_ab(LNow[0],LNow[3])>=150&&max_ab(LNow[1],LNow[2])>=80&&max_ab(LNow[4],LNow[7])>=150)
{
if(ELC_Center_14*ELC_Center_58>=0&&ELC_Center_23*ELC_Center_58>=0 &&ELC_Center_14*ELC_Center_23>=0 ){//都同号
ELC_Center0 =(int16)((float)(K1*ELC_Center_14)+(float)((K2+0.0)*ELC_Center_23));}
else if(ELC_Center_14*ELC_Center_58>=0&&ELC_Center_23*ELC_Center_58<0&&ELC_Center_14*ELC_Center_23<=0){//23异, 14,58同号
if(myabs(ELC_Center_14)>myabs(ELC_Center_58))
ELC_Center0=(int16)((float)(0.7*ELC_Center_14)+(float)(0.3*ELC_Center_58));
else
ELC_Center0=(int16)((float)(0.7*ELC_Center_58)+(float)(0.3*ELC_Center_14));}
else if(ELC_Center_14*ELC_Center_58<0&&ELC_Center_23*ELC_Center_58>=0 &&ELC_Center_14*ELC_Center_23<=0){//14异, 23,58同号
if(myabs(ELC_Center_23)>myabs(ELC_Center_58))
ELC_Center0=(int16)((float)(0.7*ELC_Center_23)+(float)(0.3*ELC_Center_58));
else
ELC_Center0=(int16)((float)(0.7*ELC_Center_58)+(float)(0.3*ELC_Center_23));}
else
{ELC_Center0=(int16)((float)(K1*ELC_Center_14)+(float)((K2+0.0)*ELC_Center_23));}
}
//14正常23不正常58正常
else if(max_ab(LNow[0],LNow[3])>=150&&max_ab(LNow[1],LNow[2])<80&&max_ab(LNow[4],LNow[7])>=150)
{
if(ELC_Center_14*ELC_Center_58>=0&&ELC_Center_23*ELC_Center_58>=0 &&ELC_Center_14*ELC_Center_23>=0 ){//都同号
if(myabs(ELC_Center_14)>myabs(ELC_Center_58))
ELC_Center0=(int16)((float)(0.7*ELC_Center_14)+(float)(0.3*ELC_Center_58));
else
ELC_Center0=(int16)((float)(0.7*ELC_Center_58)+(float)(0.3*ELC_Center_14));}
else if(ELC_Center_14*ELC_Center_58>=0&&ELC_Center_23*ELC_Center_58<0 &&ELC_Center_14*ELC_Center_23<=0){//23异, 14,58同号
if(myabs(ELC_Center_14)>myabs(ELC_Center_58))
ELC_Center0=(int16)((float)(0.7*ELC_Center_14)+(float)(0.3*ELC_Center_58));
else
ELC_Center0=(int16)((float)(0.7*ELC_Center_58)+(float)(0.3*ELC_Center_14));}
else if(ELC_Center_14*ELC_Center_58<0&&ELC_Center_23*ELC_Center_58>=0 &&ELC_Center_14*ELC_Center_23<=0){//14异, 23,58同号
ELC_Center0=(int16)((float)(1.0*ELC_Center_58));}
else
{
if(myabs(ELC_Center_14)>myabs(ELC_Center_58))
ELC_Center0=(int16)((float)(0.7*ELC_Center_14)+(float)(0.3*ELC_Center_58));
else
ELC_Center0=(int16)((float)(0.7*ELC_Center_58)+(float)(0.3*ELC_Center_14));
}
}
//14不正常23正常58正常
else if(max_ab(LNow[0],LNow[3])<150&&max_ab(LNow[1],LNow[2])>=80&&max_ab(LNow[4],LNow[7])>=150)
{
if(ELC_Center_14*ELC_Center_58>=0&&ELC_Center_23*ELC_Center_58>=0 &&ELC_Center_14*ELC_Center_23>=0){//都同号
if(myabs(ELC_Center_23)>myabs(ELC_Center_58))
ELC_Center0=(int16)((float)(0.7*ELC_Center_23)+(float)(0.3*ELC_Center_58));
else
ELC_Center0=(int16)((float)(0.7*ELC_Center_58)+(float)(0.3*ELC_Center_23));}
else if( ELC_Center_14*ELC_Center_58>=0&&ELC_Center_23*ELC_Center_58<0 &&ELC_Center_14*ELC_Center_23<=0){//23异, 14,58同号
ELC_Center0=(int16)((float)(0.7*ELC_Center_58)+(float)(0.3*ELC_Center_14));}
else if( ELC_Center_14*ELC_Center_58<0&&ELC_Center_23*ELC_Center_58>=0 &&ELC_Center_14*ELC_Center_23<=0){//14异, 23,58同号
ELC_Center0=(int16)((float)(0.7*ELC_Center_58)+(float)(0.3*ELC_Center_23));}
else
{
if(myabs(ELC_Center_23)>myabs(ELC_Center_58))
ELC_Center0=(int16)((float)(0.7*ELC_Center_23)+(float)(0.3*ELC_Center_58));
else
ELC_Center0=(int16)((float)(0.7*ELC_Center_58)+(float)(0.3*ELC_Center_23));
}
}
//14不正常23不正常58正常
else if(max_ab(LNow[0],LNow[3])<150&&max_ab(LNow[1],LNow[2])<80&&max_ab(LNow[4],LNow[7])>=150)
{
if(ELC_Center_14*ELC_Center_58>=0&&ELC_Center_23*ELC_Center_58>=0 &&ELC_Center_14*ELC_Center_23>=0 ){//都同号
ELC_Center0=(int16)((float)(0.7*ELC_Center_58)+(float)(0.2*ELC_Center_14)+(float)(0.1*ELC_Center_23));}
else if(ELC_Center_14*ELC_Center_58>=0&&ELC_Center_23*ELC_Center_58<0 &&ELC_Center_14*ELC_Center_23<=0){//23异, 14,58同号
if(myabs(ELC_Center_14)>myabs(ELC_Center_58))
ELC_Center0=(int16)((float)(0.7*ELC_Center_14)+(float)(0.3*ELC_Center_58));
else
ELC_Center0=(int16)((float)(0.7*ELC_Center_58)+(float)(0.3*ELC_Center_14));}
else if(ELC_Center_14*ELC_Center_58<0&&ELC_Center_23*ELC_Center_58>=0 &&ELC_Center_14*ELC_Center_23<=0){//14异, 23,58同号
if(myabs(ELC_Center_23)>myabs(ELC_Center_58))
ELC_Center0=(int16)((float)(0.7*ELC_Center_23)+(float)(0.3*ELC_Center_58));
else
ELC_Center0=(int16)((float)(0.7*ELC_Center_58)+(float)(0.3*ELC_Center_23));}
else
ELC_Center0=(int16)((float)(1.0*ELC_Center_58)+(float)(0.0f*ELC_Center_14)+(float)(0.0f*ELC_Center_23));
}
//14不正常23不正常58不正常
else if(max_ab(LNow[0],LNow[3])<150&&max_ab(LNow[1],LNow[2])<80&&max_ab(LNow[4],LNow[7])<150 )//14不正常23不正常58不正常
{
ELC_Center0= ELC_Center_Last;
}
}
//#include "inductance.h"
//#include <math.h>
//uint16 LNow[10];
//float LNor[10];
//uint16 LNom[10];
//uint16 LNowinit[10] = {1300,1200,1500,1200,1300,1300,1500,1300,3700,3700};
//uint16 LNORMAX[10] = {2980,2000,2000,2980,3350,1500,1500,3350,3700,3700};
//uint16 ELC[ELC_GROUP][ELC_NUM];
偏差
//int32 ELC_Center_14 = 0;
//int32 ELC_Center_23 = 0;
//int32 ELC_Center_58 = 0;
//int32 ELC_Center0 = 0;
//int32 ELC_Center1 = 0;
//int32 ELC_Center = 0;
//int32 ELC_CenterRepair = 0;
//int32 ELC_Center_Last = 0;
电感和
//uint16 ELC_Sum_14 = 0;
//uint16 ELC_Sum_23 = 0;
//uint16 ELC_Sum_58 = 0;
//uint16 ELC_Sum_1234 = 0;
//uint16 ElC_Sum_146=0;
//uint16 Fork_Sum_1234=0;
电感差
//int16 ELC_Err_14 = 0;
//int16 ELC_Err_23 = 0;
//int16 ELC_Err_58 = 0;
权重系数
//float K1=0.8f;
//float K2=0;
//float K3=0.1f;
//float K4=0.1f;
电感保护
//uint8 ELC_PROTECT_FLAG=0;
电感处理
//void Inductor_deal(void)
//{
// uint16 L_sum[ELC_NUM] = {0,0,0,0,0,0,0,0,0,0,};
// int8 i = 0,k = 0;
// uint16 ELC_max[ELC_NUM] = {4095,4095,4095,4095,4095,4095,4095,4095,4095,4095};
// uint16 ELC_min[ELC_NUM] = {0,0,0,0,0,0,0,0,0,0,};
// //读取ADC
// for(i = 0;i < ELC_GROUP ;i ++)
// {
// ELC[i][0] = adc_once(ADC1,ADC_12BIT);
// ELC[i][1] = adc_once(ADC2,ADC_12BIT);
// ELC[i][2] = adc_once(ADC3,ADC_12BIT);
// ELC[i][3] = adc_once(ADC4,ADC_12BIT);
// ELC[i][4] = adc_once(ADC5,ADC_12BIT);
// ELC[i][5] = adc_once(ADC6,ADC_12BIT);
// ELC[i][6] = adc_once(ADC7,ADC_12BIT);
// ELC[i][7] = adc_once(ADC8,ADC_12BIT);
// ELC[i][8] = adc_once(ADC9,ADC_12BIT);
// ELC[i][9] = adc_once(ADC10,ADC_12BIT);
// }
// //挑出最大最小值
// for(k = 0; k < ELC_NUM ; k++)
// {
// for(i = 0; i < ELC_GROUP; i++)
// {
// if(ELC[i][k] >= ELC_min[k])
// {
// ELC_min[k]=ELC[i][k];
// }
// if(ELC[i][k] <= ELC_max[k])
// {
// ELC_max[k]=ELC[i][k];
// }
// L_sum[k] += ELC[i][k];
// }
// }
// //第一次归一化
// if(Fork.state == Fork_begin)
// {
// for(i = 0;i < ELC_NUM;i ++)
// {
// LNow[i] = ((L_sum[i]-ELC_max[i]-ELC_min[i])/ (ELC_GROUP-2));
// LNow[i] = LNow[i]+1;
// }
// }
// else
// {
// for(i = 0;i < ELC_NUM;i ++)
// {
// LNom[i] = (L_sum[i]-ELC_max[i]-ELC_min[i])/ (ELC_GROUP-2);
// LNow[i] = 2.2*LNom[i];
// LNow[i] = LNow[i]+1;
// }
// }
// //第二次归一化
// for(i = 0;i < ELC_NUM;i++)
// {
// LNor[i] = LNow[i];
// LNor[i] = LNor[i]*100/LNORMAX[i];
// }
// //电磁保护
// if(max_ab(LNow[0],LNow[3])<= 50&&max_ab(LNow[1],LNow[2])<= 50&&max_ab(LNow[4],LNow[7])<= 500&&LNow[6]<40)
// ELC_PROTECT_FLAG = 'T';
// else
// ELC_PROTECT_FLAG = 'F';
电感值差
// ELC_Err_14 = LNor[3] - LNor[0];
// ELC_Err_23 = LNor[2] - LNor[1];
// ELC_Err_58 = LNor[7] - LNor[4];
电感值和
// ELC_Sum_14 = LNor[0] + LNor[3];
// ELC_Sum_58 = LNor[4] + LNor[7];
// ELC_Sum_23 = LNor[1] + LNor[2];
// ELC_Sum_1234= ELC_Sum_14 + ELC_Sum_23;
// Fork_Sum_1234= LNom[0] + LNom[1]+LNom[2] + LNom[3];
差比和处理
ELC_Center_14=(int16)((float)1000*(sqrt(LNor[3])-sqrt(LNor[0]))/(LNor[3]+ LNor[0]));
ELC_Center_23=(int16)((float)1000*(sqrt(LNor[2])-sqrt(LNor[1]))/(LNor[2]+ LNor[1]));
ELC_Center_58=(int16)((float)1000*(sqrt(LNor[7])-sqrt(LNor[4]))/(LNor[7]+ LNor[4]));
ELC_Center_14=(int16)((float)1400*(LNor[3]+LNor[2]-(LNor[0]+LNor[1]))/(LNor[3]+LNor[5]+ LNor[0]+LNor[1]+ LNor[2]));
ELC_Center_23=(int16)((float)1000*(sqrt(LNor[2])-sqrt(LNor[1]))/(LNor[2]+ LNor[1]));
ELC_Center_58=(int16)((float)1400*(sqrt(LNor[7])-sqrt(LNor[4]))/(LNor[7]+LNor[6]+LNor[4]));
ELC_Center_23=(int16)((float)100*(LNor[2]-LNor[1])/(LNor[2]+ LNor[1]));
ELC_Center_14=(int32)((float)900*(LNor[3]-LNor[0])/(LNor[3]*LNor[0]));
ELC_Center_23=(int32)((float)700*(LNor[2]-LNor[1])/(LNor[2]*LNor[1]));
ELC_Center_58=(int32)((float)800*(LNor[7]-LNor[4])/(LNor[7]*LNor[4]));
ELC_Center_14=(int32)((float)2000*(sqrt(LNor[3])-sqrt(LNor[0]))/(LNor[3]+1.5*LNor[5]+ LNor[0]));
ELC_Center_23=(int32)((float)1500*(sqrt(LNor[2])-sqrt(LNor[1]))/(LNor[2]+ LNor[1]));
// ELC_Center_14=(int32)((float)1400*(sqrt(LNor[3])-sqrt(LNor[0]))/(LNor[3]+LNor[5]+ LNor[0]));
// ELC_Center_23=(int32)((float)1000*(sqrt(LNor[2])-sqrt(LNor[1]))/(LNor[2]+ LNor[1]));
// ELC_Center_58=(int32)((float)1800*(sqrt(LNor[7])-sqrt(LNor[4]))/(LNor[7]+LNor[4]));
系数修正
// K2=(float)myabs(6.5f*(limit_ab(ELC_Center_14,-100.0f,100.0f)/1000.0f));
// K1=(float)(1.30f-K2);
ELC_Center1=(int32)((float)120*(0.4*(LNor[3]-LNor[0])+1.2*(LNor[2]-LNor[1]))/(0.4*(LNor[3]+ LNor[0])+LNor[5]+1.0f*myabs(LNor[2]-LNor[1])));
ELC_Center1=(int32)((float)1650*(K1*(sqrt(LNor[3])-sqrt(LNor[0]))+ K2*(sqrt(LNor[2])-sqrt(LNor[1])))/(0.4*(LNor[3]+ LNor[0])+1.5*LNor[5]+0.8f*myabs(LNor[2]-LNor[1])));
ELC_Center1=(int32)((float)1650*(0.4*(sqrt(LNor[3])-sqrt(LNor[0]))+(sqrt(LNor[2])-sqrt(LNor[1])))/(0.4*(LNor[3]+ LNor[0])+1.5*LNor[5]+0.8f*myabs(LNor[2]-LNor[1])));
ELC_Center0= (int32)((float)1650*(K1*(sqrt(LNor[3])-sqrt(LNor[0]))+(K2+0.2f)*(sqrt(LNor[2])-sqrt(LNor[1])))/(K1*(LNor[3]+ LNor[0])+1.5*LNor[5]+(K2+0.2)*myabs(LNor[2]-LNor[1])));
ELC_Center0= (int32)((float)1650*(K1*(sqrt(LNor[3])-sqrt(LNor[0]))+(K2+0.2f)*(sqrt(LNor[2])-sqrt(LNor[1])))/(K1*(LNor[3]+ LNor[0])+1.5*LNor[5]+(K2+0.2)*myabs(LNor[2]-LNor[1])));
ELC_Center1= (int32)((float)2300*(K1*(sqrt(LNor[3])-sqrt(LNor[0]))+(K2+0.2f)*(sqrt(LNor[2])-sqrt(LNor[1])))/(K1*sqrt((LNor[3]*LNor[0]))+1.5*LNor[5]+(K2+0.2)*(LNor[1]+LNor[2])));
ELC_Center0= (int32)((float)2200*(K1*(sqrt(LNor[3])-sqrt(LNor[0]))+(K2+0.2f)*(sqrt(LNor[2])-sqrt(LNor[1])))/(K1*sqrt((LNor[3]*LNor[0]))+1.5*LNor[5]+(K2+0.2)*(LNor[1]+LNor[2])));
K3=(float)myabs(5.5f*(limit_ab(ELC_Center0,-100.0f,100.0f)/1000.0f));
K4=1.0-K3;
ELC_Center1= K4*ELC_Center0+K3*ELC_Center_23;
// Elc_boundary_deal();
ELC_Center0=ELC_Center_58;
// ELC_Center1=ELC_Center0;
///*特殊元素,处理*/
// if(Cross.FindFlag == 'T')
// {
// if(Cross.state==Crossin)
// {
// ELC_Center1=0.6*ELC_Center0;
// }
// else if(Cross.state==Crosson)
// {
// ELC_Center1= ELC_Center0;
// }
// }
if(Fork.Dir == 'L'&&Fork.state == Fork_middle){ELC_Center1=0.6*ELC_Center_58+0.4*ELC_Center_14;}
if(max_ab(LNow[0],LNow[3])>=150&&max_ab(LNow[1],LNow[2])<80&&max_ab(LNow[4],LNow[7])>=150)
{
if(ELC_Center_14*ELC_Center_58>=0&&ELC_Center_23*ELC_Center_58>=0 &&ELC_Center_14*ELC_Center_23>=0 ){//都同号
if(myabs(ELC_Center_14)>myabs(ELC_Center_23))
ELC_Center1=(int16)((float)(1.2*ELC_Center_14)+(float)(0.3*ELC_Center_23));
else
ELC_Center1=(int16)((float)(1.2*ELC_Center_23)+(float)(0.3*ELC_Center_14));}
else if(ELC_Center_14*ELC_Center_58>=0&&ELC_Center_23*ELC_Center_58<0 &&ELC_Center_14*ELC_Center_23<=0){//23异, 14,58同号
if(myabs(ELC_Center_14)>myabs(ELC_Center_58))
ELC_Center1=(int16)((float)(1.2*ELC_Center_14)+(float)(0.3*ELC_Center_58));
else
ELC_Center1=(int16)((float)(1.2*ELC_Center_58)+(float)(0.3*ELC_Center_14));}
else if(ELC_Center_14*ELC_Center_58<0&&ELC_Center_23*ELC_Center_58>=0 &&ELC_Center_14*ELC_Center_23<=0){//14异, 23,58同号
if(myabs(ELC_Center_23)>myabs(ELC_Center_58))
ELC_Center1=(int16)((float)(1.2*ELC_Center_23)+(float)(0.3*ELC_Center_58));
else
ELC_Center1=(int16)((float)(1.2*ELC_Center_58)+(float)(0.3*ELC_Center_23));}
else
{
if(myabs(ELC_Center_14)>myabs(ELC_Center_23))
ELC_Center1=(int16)((float)(1.2*ELC_Center_14)+(float)(0.3*ELC_Center_23));
else
ELC_Center1=(int16)((float)(1.2*ELC_Center_23)+(float)(0.3*ELC_Center_14));
}
}
else if(max_ab(LNow[0],LNow[3])<150&&max_ab(LNow[1],LNow[2])>=80&&max_ab(LNow[4],LNow[7])>=150)
{
if(ELC_Center_14*ELC_Center_58>=0&&ELC_Center_23*ELC_Center_58>=0 &&ELC_Center_14*ELC_Center_23>=0 ){//都同号
if(myabs(ELC_Center_23)>myabs(ELC_Center_14))
ELC_Center1=(int16)((float)(1.2*ELC_Center_23)+(float)(0.3*ELC_Center_14));
else
ELC_Center1=(int16)((float)(1.2*ELC_Center_14)+(float)(0.3*ELC_Center_23));}
else if(ELC_Center_14*ELC_Center_58>=0&&ELC_Center_23*ELC_Center_58<0 &&ELC_Center_14*ELC_Center_23<=0){//23异, 14,58同号
if(myabs(ELC_Center_14)>myabs(ELC_Center_58))
ELC_Center1=(int16)((float)(1.2*ELC_Center_14)+(float)(0.3*ELC_Center_58));
else
ELC_Center1=(int16)((float)(1.2*ELC_Center_58)+(float)(0.3*ELC_Center_14));}
else if(ELC_Center_14*ELC_Center_58<0&&ELC_Center_23*ELC_Center_58>=0 &&ELC_Center_14*ELC_Center_23<=0){//14异, 23,58同号
if(myabs(ELC_Center_23)>myabs(ELC_Center_58))
ELC_Center1=(int16)((float)(1.2*ELC_Center_23)+(float)(0.3*ELC_Center_58));
else
ELC_Center1=(int16)((float)(1.2*ELC_Center_58)+(float)(0.3*ELC_Center_23));}
else
{
if(myabs(ELC_Center_14)>myabs(ELC_Center_23))
ELC_Center1=(int16)((float)(1.2*ELC_Center_14)+(float)(0.3*ELC_Center_23));
else
ELC_Center1=(int16)((float)(1.2*ELC_Center_23)+(float)(0.3*ELC_Center_14));
}
}
else if(max_ab(LNow[0],LNow[3])<150&&max_ab(LNow[1],LNow[2])<80&&max_ab(LNow[4],LNow[7])>=150)
{
if(ELC_Center_14*ELC_Center_58>=0&&ELC_Center_23*ELC_Center_58>=0 &&ELC_Center_14*ELC_Center_23>=0 ){//都同号
ELC_Center1=(int16)((float)(2.0*ELC_Center_58));}
else
ELC_Center1 = ELC_Center_Last;
// if(myabs(ELC_Center_23)>myabs(ELC_Center_14))
// ELC_Center1=(int16)((float)(1.2*ELC_Center_23)+(float)(0.3*ELC_Center_14));
// else
// ELC_Center1=(int16)((float)(1.2*ELC_Center_14)+(float)(0.3*ELC_Center_23));}
// else if(ELC_Center_14*ELC_Center_58>=0&&ELC_Center_23*ELC_Center_58<0 &&ELC_Center_14*ELC_Center_23<=0){//23异, 14,58同号
// if(myabs(ELC_Center_14)>myabs(ELC_Center_58))
// ELC_Center1=(int16)((float)(1.2*ELC_Center_14)+(float)(0.3*ELC_Center_58));
// else
// ELC_Center1=(int16)((float)(1.2*ELC_Center_58)+(float)(0.3*ELC_Center_14));}
// else if(ELC_Center_14*ELC_Center_58<0&&ELC_Center_23*ELC_Center_58>=0 &&ELC_Center_14*ELC_Center_23<=0){//14异, 23,58同号
// if(myabs(ELC_Center_23)>myabs(ELC_Center_58))
// ELC_Center1=(int16)((float)(1.2*ELC_Center_23)+(float)(0.3*ELC_Center_58));
// else
// ELC_Center1=(int16)((float)(1.2*ELC_Center_58)+(float)(0.3*ELC_Center_23));}
// else
// {
// if(myabs(ELC_Center_14)>myabs(ELC_Center_23))
// ELC_Center1=(int16)((float)(1.2*ELC_Center_14)+(float)(0.3*ELC_Center_23));
// else
// ELC_Center1=(int16)((float)(1.2*ELC_Center_23)+(float)(0.3*ELC_Center_14));
// }
}
else if(max_ab(LNow[0],LNow[3])<150&&max_ab(LNow[1],LNow[2])<80&&max_ab(LNow[4],LNow[7])<150)
{
ELC_Center1 = ELC_Center_Last;
}
// ELC_Center=KalmanFilter(&KFP_ELCCenter,ELC_Center1);
// ELC_Center_Last = ELC_Center;
//}
//
//
//void Elc_boundary_deal(void)
//{
// //14正常23正常58正常
// if(max_ab(LNow[0],LNow[3])>=150&&max_ab(LNow[1],LNow[2])>=80&&max_ab(LNow[4],LNow[7])>=150)
// {
// if(ELC_Center_14*ELC_Center_58>=0&&ELC_Center_23*ELC_Center_58>=0 &&ELC_Center_14*ELC_Center_23>=0 ){//都同号
// ELC_Center0 =(int16)((float)(K1*ELC_Center_14)+(float)(K2*ELC_Center_23));}
// else if(ELC_Center_14*ELC_Center_58>=0&&ELC_Center_23*ELC_Center_58<0&&ELC_Center_14*ELC_Center_23<=0){//23异, 14,58同号
// if(myabs(ELC_Center_14)>myabs(ELC_Center_58))
// ELC_Center0=(int16)((float)(0.7*ELC_Center_14)+(float)(0.3*ELC_Center_58));
// else
// ELC_Center0=(int16)((float)(0.7*ELC_Center_58)+(float)(0.3*ELC_Center_14));}
// else if(ELC_Center_14*ELC_Center_58<0&&ELC_Center_23*ELC_Center_58>=0 &&ELC_Center_14*ELC_Center_23<=0){//14异, 23,58同号
// if(myabs(ELC_Center_23)>myabs(ELC_Center_58))
// ELC_Center0=(int16)((float)(0.7*ELC_Center_23)+(float)(0.3*ELC_Center_58));
// else
// ELC_Center0=(int16)((float)(0.7*ELC_Center_58)+(float)(0.3*ELC_Center_23));}
//
// else
// {ELC_Center0=(int16)((float)(K1*ELC_Center_14)+(float)(K2*ELC_Center_23));}
// }
// //14正常23不正常58正常
// else if(max_ab(LNow[0],LNow[3])>=150&&max_ab(LNow[1],LNow[2])<80&&max_ab(LNow[4],LNow[7])>=150)
// {
// if(ELC_Center_14*ELC_Center_58>=0&&ELC_Center_23*ELC_Center_58>=0 &&ELC_Center_14*ELC_Center_23>=0 ){//都同号
// if(myabs(ELC_Center_14)>myabs(ELC_Center_58))
// ELC_Center0=(int16)((float)(0.7*ELC_Center_14)+(float)(0.3*ELC_Center_58));
// else
// ELC_Center0=(int16)((float)(0.7*ELC_Center_58)+(float)(0.3*ELC_Center_14));}
// else if(ELC_Center_14*ELC_Center_58>=0&&ELC_Center_23*ELC_Center_58<0 &&ELC_Center_14*ELC_Center_23<=0){//23异, 14,58同号
// if(myabs(ELC_Center_14)>myabs(ELC_Center_58))
// ELC_Center0=(int16)((float)(0.7*ELC_Center_14)+(float)(0.3*ELC_Center_58));
// else
// ELC_Center0=(int16)((float)(0.7*ELC_Center_58)+(float)(0.3*ELC_Center_14));}
// else if(ELC_Center_14*ELC_Center_58<0&&ELC_Center_23*ELC_Center_58>=0 &&ELC_Center_14*ELC_Center_23<=0){//14异, 23,58同号
// if(myabs(ELC_Center_23)>myabs(ELC_Center_58))
// ELC_Center0=(int16)((float)(0.7*ELC_Center_23)+(float)(0.3*ELC_Center_58));
// else
// ELC_Center0=(int16)((float)(0.7*ELC_Center_58)+(float)(0.3*ELC_Center_23));}
//
// else
// {
// if(myabs(ELC_Center_14)>myabs(ELC_Center_23))
// ELC_Center0=(int16)((float)(0.7*ELC_Center_14)+(float)(0.3*ELC_Center_23));
// else
// ELC_Center0=(int16)((float)(0.7*ELC_Center_23)+(float)(0.3*ELC_Center_14));
// }
// }
// //14不正常23正常58正常
// else if(max_ab(LNow[0],LNow[3])<150&&max_ab(LNow[1],LNow[2])>=80&&max_ab(LNow[4],LNow[7])>=150)
// {
// if(ELC_Center_14*ELC_Center_58>=0&&ELC_Center_23*ELC_Center_58>=0 &&ELC_Center_14*ELC_Center_23>=0){//都同号
// if(myabs(ELC_Center_23)>myabs(ELC_Center_58))
// ELC_Center0=(int16)((float)(0.7*ELC_Center_23)+(float)(0.3*ELC_Center_58));
// else
// ELC_Center0=(int16)((float)(0.7*ELC_Center_58)+(float)(0.3*ELC_Center_23));}
// else if( ELC_Center_14*ELC_Center_58>=0&&ELC_Center_23*ELC_Center_58<0 &&ELC_Center_14*ELC_Center_23<=0){//23异, 14,58同号
// if(myabs(ELC_Center_14)>myabs(ELC_Center_58))
// ELC_Center0=(int16)((float)(0.7*ELC_Center_14)+(float)(0.3*ELC_Center_58));
// else
// ELC_Center0=(int16)((float)(0.7*ELC_Center_58)+(float)(0.3*ELC_Center_14));}
// else if( ELC_Center_14*ELC_Center_58<0&&ELC_Center_23*ELC_Center_58>=0 &&ELC_Center_14*ELC_Center_23<=0){//14异, 23,58同号
// if(myabs(ELC_Center_23)>myabs(ELC_Center_58))
// ELC_Center0=(int16)((float)(0.7*ELC_Center_23)+(float)(0.3*ELC_Center_58));
// else
// ELC_Center0=(int16)((float)(0.7*ELC_Center_58)+(float)(0.3*ELC_Center_23));}
// else
// {
// if(myabs(ELC_Center_23)>myabs(ELC_Center_58))
// ELC_Center0=(int16)((float)(0.7*ELC_Center_23)+(float)(0.3*ELC_Center_58));
// else
// ELC_Center0=(int16)((float)(0.7*ELC_Center_58)+(float)(0.3*ELC_Center_23));
// }
// }
// //14不正常23不正常58正常
// else if(max_ab(LNow[0],LNow[3])<150&&max_ab(LNow[1],LNow[2])<80&&max_ab(LNow[4],LNow[7])>=150)
// {
// if(ELC_Center_14*ELC_Center_58>=0&&ELC_Center_23*ELC_Center_58>=0 &&ELC_Center_14*ELC_Center_23>=0 ){//都同号
// ELC_Center0=(int16)((float)(0.7*ELC_Center_58)+(float)(0.2*ELC_Center_14)+(float)(0.1*ELC_Center_23));}
// else if(ELC_Center_14*ELC_Center_58>=0&&ELC_Center_23*ELC_Center_58<0 &&ELC_Center_14*ELC_Center_23<=0){//23异, 14,58同号
// if(myabs(ELC_Center_14)>myabs(ELC_Center_58))
// ELC_Center0=(int16)((float)(0.7*ELC_Center_14)+(float)(0.3*ELC_Center_58));
// else
// ELC_Center0=(int16)((float)(0.7*ELC_Center_58)+(float)(0.3*ELC_Center_14));}
// else if(ELC_Center_14*ELC_Center_58<0&&ELC_Center_23*ELC_Center_58>=0 &&ELC_Center_14*ELC_Center_23<=0){//14异, 23,58同号
// if(myabs(ELC_Center_23)>myabs(ELC_Center_58))
// ELC_Center0=(int16)((float)(0.7*ELC_Center_23)+(float)(0.3*ELC_Center_58));
// else
// ELC_Center0=(int16)((float)(0.7*ELC_Center_58)+(float)(0.3*ELC_Center_23));}
// else
// ELC_Center0= ELC_Center_Last;
// }
// //14不正常23不正常58不正常
// else if(max_ab(LNow[0],LNow[3])<150&&max_ab(LNow[1],LNow[2])<80&&max_ab(LNow[4],LNow[7])<150 )//14不正常23不正常58不正常
// {
// ELC_Center0= ELC_Center_Last;
// }
//}
//
//
//
//
//
五、元素识别
元素识别就是对磁场叠加的理解,和电感排布的选择,
比如,环岛,有几个特征:
1.中间是四根线的汇合,这一点电感值比直道大。
2.抽象地与普通直道相比,在赛道地一侧加了弯曲地铜线。
3.环岛中就是正常的弯道。
三岔:1.入口处,两根线分开了,每个三岔路都只有一根线,入口处电感值减少。
2.出口处,一根线变为两根线,电感值变大。
十字:
1.水平电感的值基本不受影响
2.垂直电感可以判到与前进方向交叉铜线
3. 根据磁场的叠加,如果把十字看成坐标轴,分成四块看成四个象限,那么四个象限磁场的叠加是不同的,45°电感可以测出这种区别
只是简单说一下,元素这一块有着自己的理解,调车的时候也方便,
具体的元素判断可以参考网上一些好的教程,智能车电磁组——环岛处理_彭小宴的博客-CSDN博客_智能车电磁环岛处理方法
这个需要多上网查阅相关资料。
六、代码结构
#include "headfile.h"
void main()
{
DisableGlobalIRQ();
sys_clk = 30000000;
board_init(); //初始化寄存器
初始化函数;
EnableGlobalIRQ();
while(1)
{
杂项
}
}
void TM4_Isr() interrupt 20
{
TIM4_CLEAR_FLAG; //清除中断标志
信息采集;(编码器,陀螺仪,电感值)
信息加工;(元素/状态判断)
信息处理;(电机舵机控制等)
}
七、停车保护
在调试车的时候,有时候出现车冲出赛道乱跑的情况。
因此我们要写保护程序,让车停止下来,防止车受损、人受伤。
void SpeedControl(void)
{
EncoderRead();
if (CarInfo.Protect_Flag == 'F') {
// if(Wheel_L.SpeedNow-ExSpeed>50){SpeedOut_L=-999;}//梆梆算法
// else if(Wheel_R.SpeedNow-ExSpeed>50)SpeedOut_R =-999;
// else
// {
/*正常情况*/
SpeedOut_L = SimpleIncremental_PID(&SpeedPIDParam[0][0],&SpeedPIDLeft,ExSpeed_L,Wheel_L.SpeedNow);
SpeedOut_R = SimpleIncremental_PID(&SpeedPIDParam[0][0],&SpeedPIDRight,ExSpeed_R,Wheel_R.SpeedNow);
/*特殊情况*/
if (BackGarage.FindFlag=='T') {
SpeedOut_L = SimpleIncremental_PID(&SpeedPIDParam[1][0],&SpeedPIDLeft,ExSpeed_L,Wheel_L.SpeedNow);
SpeedOut_R = SimpleIncremental_PID(&SpeedPIDParam[1][0],&SpeedPIDRight,ExSpeed_R,Wheel_R.SpeedNow);
}
// }
}
else if (CarInfo.Protect_Flag == 'T'){//速度控零保护
if (myabs(Wheel_L.SpeedNow)>= 400 || myabs(Wheel_R.SpeedNow) >= 400) {
SpeedOut_L = 0;
SpeedOut_R = 0;
}
else {
SpeedOut_L = SimpleIncremental_PID(&SpeedPIDParam[0][0],&SpeedPIDLeft,0,Wheel_L.SpeedNow);
SpeedOut_R =SimpleIncremental_PID(&SpeedPIDParam[0][0],&SpeedPIDRight,0,Wheel_R.SpeedNow);
}
}
pwm_l = SpeedOut_L;
pwm_r = SpeedOut_R;
MotorWrite();
}
下面我给出几种触发思路:
1.当车原理赛道,电感值就会骤减,电感电容对的值小于一些值的时候,就触发。为了防止误触,就加一个标志位,如代码所示。
//电磁保护
if(max_ab(LNow[0],LNow[3])<= 10&&max_ab(LNow[1],LNow[2])<= 10&&LNow[6]<10&&max_ab(LNow[4],LNow[7])<= 10)
ELC_PROTECT_FLAG = 'T';
else
ELC_PROTECT_FLAG = 'F';
void Car_Protect(void)
{
//---------------电磁保护--------------//
if(((ELC_PROTECT_FLAG == 'T'&&OutGarage.Flag == 'T')&&Ramp.FindFlag == 'F')&&BackGarage.FindFlag=='F')
{
CarOutProtectCnt ++;
}
else
{
CarOutProtectCnt=0;
}
if(CarOutProtectCnt >=50)
{
CarInfo.Protect_Flag = 'T';
}
}
2.如果用的这种microusb安卓充电线供电的电磁信号发生器,
就加一个带开关的USB延长线,当想让车停下来的时候,就切换开关,使信号发生器断电,程序参考上面的。
3.通过串口。车加上无线串口模块,当串口收到信息或者指定内容时触发停车保护。也可以用蓝牙模块,手机下一个蓝牙控制app,就可以发送数据让车停止。
//UART3中断
void UART3_Isr() interrupt 17
{
if(UART3_GET_TX_FLAG)
{
UART3_CLEAR_TX_FLAG;
busy[3] = 0;
}
if(UART3_GET_RX_FLAG)
{
UART3_CLEAR_RX_FLAG;
// CarInfo.Protect_Flag = 'T';/保护
//接收数据寄存器为:S3BUF
if(wireless_type == WIRELESS_SI24R1)
{
wireless_uart_callback(); //无线转串口回调函数
}
else if(wireless_type == WIRELESS_CH9141)
{
bluetooth_ch9141_uart_callback(); //蓝牙转串口回调函数
}
else if(wireless_type == WIRELESS_CH573)
{
wireless_ch573_callback(); //CH573无线模块回调函数
}
}
}
八、内容补充
智能车入门补充篇——电感值处理、转向控制与巡线-CSDN博客
系列文章目录
文章分为三个层次
速通版
是希望通过简化的步骤搭建出寻迹小车,进而了解整个智能车是如何实现的,快速上手,为后续参与智能车竞赛做基础。
如果只是为了完成学校智能车初期培训,做出能简单循迹的小车,可以看这个速通版。
全程引导篇
是讲了做出能够完赛的智能车的整个过程,大部分文章只是简单点拨一下,但是附上了相关的文章链接,方便根据自己的情况深入了解。
全程引导篇,能够带你比较系统地了解整个智能车的制作过程,推荐备赛初期或者有车模之后学习。
详细讲解篇
是全程引导篇的补充,由于全程引导篇是引导性质,文章内容只是点拨,缺乏相应的原理或代码讲解,因此写详细讲解篇作为补充。
详细讲解篇会渗透在全程引导篇中。
速通版
全程引导篇
详细讲解篇
智能车模块详解——数据存储与读写(eeprom/flash)
(更新中)