冒泡法
冒泡法是一种简单的排序算法，它重复地遍历要排序的数列，一次比较两个元素，如果它们的顺序错误就把它们交换过来。遍历数列的工作是重复地进行直到没有再需要交换，也就是说该数列已经排序完成。冒泡排序的时间复杂度为O(n^2)。
//排序 四路ADC
for(i = 0;i < ELC_GROUP;i ++)//所有的组都需要排序
{
for(k = 0;k < ELC_NUM;k ++)//每个电感号组内排序
{
len = ELC_TIME;
while(len > 1)//排序到只剩下一个元素后不再进行排序
{
L_max = 0;
L_max_index = 0;
for(j = 0;j < len;j ++)
{
if(ELC[i][j][k] > L_max)
{
L_max = ELC[i][j][k];//记录最大值
L_max_index = j;//记录最大值下标
}
}
ELC[i][L_max_index][k] = ELC[i][len - 1][k];//交换数据
ELC[i][len - 1][k] = L_max;//最大值后移
len --;
}
L_sum[k] += ELC[i][ELC_NEED][k];
}
}归一化
归一化是一种数据预处理方法，用于将不同规模的数据转换为同一规模的数据。常见的归一化方法包括最小-最大归一化和z-score归一化。最小-最大归一化将数据缩放到(0,1]的范围内，z-score归一化将数据转换为均值为0，标准差为1的分布。
注意，归一化的时候尽量避免归到0，否则当0做分母的时候会带来数据处理上的麻烦。
//第一次归一化
for(i = 0;i < ELC_NUM;i++)
{
LNor[i] = LNow[i];
LNor[i] = LNor[i]*100/LNORMAX[i];
}
//电磁保护
if(ELC_Sum_14 <= 55)
ELC_PROTECT_FLAG = 'T';
else
ELC_PROTECT_FLAG = 'F';
//复制原归一化值
// LNorm[0] = LNor[0] + 1;
// LNorm[1] = LNor[1] + 1;
// LNorm[2] = LNor[2] + 1;
// LNorm[3] = LNor[3] + 1;
// for(i = 0; i < ELC_NUM; i++) {
// LNow[i] = LNow[i] + 1;
// }
// LNow[0] = 100*(1150-LNow[0])/(900);
// LNow[1] = LNow[1] + 1;
// LNow[2] = LNow[2] + 1;
// LNow[3] = 100*(910-LNow[3])/650;
LNow[0] = LNow[0]+1;
LNow[1] = LNow[1] + 1;
LNow[2] = LNow[2] + 1;
LNow[3] = LNow[3]+1;滤波
滤波是一种信号处理方法，用于去除信号中的噪声或不需要的成分。常见的滤波方法包括低通滤波、高通滤波、带通滤波和带阻滤波等。滤波的实现可以使用数字滤波器或模拟滤波器。
卡尔曼滤波是一种用于估计系统状态的滤波方法，它可以通过对系统的测量值和模型进行融合来提高状态估计的精度。卡尔曼滤波的核心思想是通过对系统状态的预测和测量值的校正来不断更新状态估计值。卡尔曼滤波广泛应用于导航、控制、信号处理等领域。
差比和加权算法
差比和
- 读取左右两个电磁感应模块的电磁信号值；
- 对左右两个电磁感应模块的电磁信号值进行差分处理，得到一个差值；
- 根据差值的正负来判断小车是否偏离了中线；
- 根据差值的大小来调整小车的转向角度，使其重新回到中线上。
加权
加权算法是一种常用的循迹算法，它通过对左右两个电磁感应模块的电磁信号进行加权处理，得到一个加权值，用来判断小车是否偏离了中线。根据加权值的大小来调整小车的转向角度，使其重新回到中线上。
直接调节电感差比和差加权算法可能会因无法直观的了解各参数而走弯路，建议先调出一套相对稳定的电感差比和加权方案，然后将其改写为电感差比和差加权算法。
使用差比和差加权算法时，小车在弯道道型出现小幅度过调，此时轻微降低比例系数即可完全拟合。
//14 23电感作差
LError[0] = LNow[3]- LNow[0];//横电感值差
LError[1] = LNow[2]- LNow[1];//斜电感值差
//14 23电感做和
LSum[0] = LNow[3] + LNow[0];//横电感值和
LSum[1] = LNow[2] + LNow[1];//横电感值和
//差比和计算两种中线
// ELC_Center_14 = LError[0] * (409600/63) / LSum[0];//横电感
ELC_Center_14 = LError[0] * 4096 / pow( LSum[0], 1.5);//横电感
// ELC_Center_23 = LError[1] * 4096 / pow( LSum[1], 1.5);//横电感
// ELC_Center_23 = LError[1] * 100 / LSum[1];//横电感
// ELC_Center_1234=100*(0.80*LError[0]+0.20*LError[1])/(0.80*LSum[0]+0.20*LSum[1]);
//中线限幅 200
// ELC_Center_14 = limit_ab(ELC_Center_14,-100,100);
// ELC_Center_23 = limit_ab(ELC_Center_23,-100,100);
ELC_Center=ELC_Center_14;※电感值处理代码
下面是我当时用的处理方案。
#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;
// }
//}
//
//
//
//
// 经典PID
• PID控制器是一种常见的控制器，它可以通过对系统的反馈信号进行处理，来控制系统的输出。在PID控制器中，P代表比例控制，I代表积分控制，D代表微分控制。
• 比例、积分、微分控制，简称PID控制，又称PID调节。PID控制的基本方法是根据系统输入与预定输出的偏差的大小运用比例、积分、微分计算出一个控制量，将这个控制量输入系统，获得输出量，通过反馈回路再次检测该输出量的偏差，循环上述过程，以使输出达到预定值。 PID控制器问世至今已有近 70年历史，其有着结构简单、稳定性好、工作可靠、调整方便等优点。
当被控对象的结构和参数不能完全掌握，或得不到精确的数学模型时，控制理论的其它技术难以采用时，系统控制器的结构和参数必须依靠经验和现场调试来确定，这时应用PID控制技术最为方便。即当我们不完全了解一个系统和被控对象，或不能通过有效的测量手段来获得系统参数时，也适合用PID控制技术。
• 连续型PID的公式：
即对err(t)分别进行比例，积分，微分运算之后进行加权求和得到输出U(t)。
• 由于单片机控制是采样控制，只能根据采样时刻的偏差控制输出量而不能计算连续输出量，所以要采用离散化的 PID算法。离散数字 PID分为位置式和增量式两种。
而增量式PID算法对应可由位置式推出：
具体的代码实现为：
float Realize_PID(PIDTypeDef_t *pid, float get, float set) {
pid->Now_Val = get;
pid->Ex_Val = set;
pid->Err_Now = pid->Ex_Val - pid->Now_Val;
pid->Output_p = pid->Kp * pid->Err_Now;
pid->Output_i += pid->Ki * pid->Err_Now;
pid->Output_d = pid->Kd * (pid->Err_Now - pid->Err_Last);
pid->Output = pid->Output_p + pid->Output_i + pid->Output_d;
pid->Err_Last = pid->Err_Now;
return pid->Output;
}
float Incremental_PID(PIDTypeDef_t *pid, float get, float set) {
pid->Now_Val = get;
pid->Ex_Val = set;
pid->Err_Now = pid->Ex_Val - pid->Now_Val;
pid->Output_p = pid->Kp * (pid->Err_Now - pid->Err_Last);
pid->Output_i = pid->Ki * pid->Err_Now;
pid->Output_d = pid->Kd * (pid->Err_Now - 2.000f * pid->Err_Last + pid->Err_LastLast);
pid->Output += pid->Output_p + pid->Output_i + pid->Output_d;
pid->Err_Last = pid->Err_Now;
pid->Err_LastLast = pid->Err_Last;
return pid->Output;
}
• 公式中没有体现周期T，其实考虑到周期是一个常数，就直接合在了系数中。采样周期和控制周期都会影响系数。
• 在PID算法中，比例环节 P 的作用是成比例地反映控制系统的偏差信号e(t)，一旦产生偏差，比例控制环节立即产生控制作用以减小偏差；积分环节 I 的作用是消除静差，提高系统的无差度。微分环节 D 的作用是反映偏差信号的变化趋势，能够在偏差信号变化之前先引入一个有效的早期修正信号来提前修正偏差，加快系统的动作速度，减少调节时间。
• 所谓增量式PID 是指数字控制器的输出只是控制量的增量△u(k)。增量式PID优点在于，由于输出的是增量所以误动作时影响小，必要时可用逻辑判断的方法关掉。另外由于控制增量△u(k)的确定仅与最近k次的采样值有关，所以较容易通过加权处理而获得比较好的控制效果。
• 在PID参数的选择上，公认的常规方案是，过程控制系统（如电机控制）因较慢的响应常采用PI控制，突出积分项的作用，而随动控制系统（如舵机控制）为了达到较快的响应常采用PD控制。
• 大多智能车队会推荐速度环使用增量式PID，方向环使用位置式PID，这是由于增量PID由于也不容易造成电机的正反转切换。但我们认为，在合适的参数下，增量式PID与位置式PID应该是可以等价。
改进式PID
死区
死区是一种常见的控制器设计技术，它可以避免系统在小范围内的震荡。具体来说，死区是指在控制器输出的范围内设置一个区域，当系统的输出在这个区域内时，控制器不进行调整，从而避免了系统的震荡。
变速积分
变速积分是一种针对系统存在积分饱和的情况下的PID控制器。它通过对积分项进行变速，来避免积分项在系统运动过程中出现饱和的情况。具体来说，变速积分会根据系统的运动状态来调整积分项的速度。
有效偏差法
有效偏差法是一种针对系统存在偏差的情况下的PID控制器。它通过对偏差进行处理，来提高系统的响应速度和稳定性。具体来说，有效偏差法会根据系统的运动状态来调整偏差项的大小。
遇限削弱积分
遇限削弱积分是一种针对系统存在积分饱和的情况下的PID控制器。它通过对积分项进行处理，来避免积分项在系统运动过程中出现饱和的情况。具体来说，遇限削弱积分会根据系统的运动状态来调整积分项的大小。
专家PID
专家PID是一种针对复杂系统的PID控制器。它通过对系统的特性进行分析，来设计出最优的PID控制器。具体来说，专家PID会根据系统的特性来调整PID控制器的参数。
if (pid->Err_ABS < S_Err) {
//小偏差 较小误差, 调小比例
pid->Output = Offine_K * pid->Output;
} else if (pid->Err_ABS > L_Err) {
//大偏差
//串联BangBang-PID
pid->Output = (pid->Err_Now > 0) ? (9500) : (-9500);
} else {
//正常偏差
if ((pid->Err_Now * (pid->Err_Now - pid->Err_Last) > 0 && pid->Err_Last * (pid->Err_Last - pid->Err_LastLast) > 0) || (pid->Err_Now - pid->Err_Last) == 0) {
//偏差在朝向偏差绝对值增大的方向变化(偏差越来越大), 或者偏差一直为某一固定值
if (pid->Err_ABS > Mid_Err) {
//控制器实施较强的控制作用
pid->Output = Online_K * pid->Output;
} else {
//但是偏差绝对值本身并不是很大
pid->Output = pid->Output + 0;
}
} else if(pid->Err_Now * (pid->Err_Now - pid->Err_Last) < 0 && (pid->Err_Now - pid->Err_Last) * (pid->Err_Last - pid->Err_LastLast) > 0 || (pid->Err_Now == 0 && pid->Err_Last == 0) {
//偏差的绝对值向减小的方向变化，或者已经达到平衡状态
//此时可以保持控制器输出不变
} else if(pid->Err_Now * (pid->Err_Now - pid->Err_Last) < 0 && ((pid->Err_Now - pid->Err_Last) * (pid->Err_Last - pid->Err_LastLast) < 0)) {
//偏差处于极值极限状态
if (pid->Err_ABS > Mid_Err) {
pid->Output = Online_K * pid->Output;
} else {
//但是偏差绝对值本身并不是很大
pid->Output = pid->Output + 0;
}
} else {
//正常情况
pid->Output = pid->Output + 0;
}
}
模糊PID
模糊PID是一种针对复杂系统的PID控制器。它通过对系统的特性进行模糊化处理，来设计出最优的PID控制器。具体来说，模糊PID会根据系统的特性来调整PID控制器的参数。
//------------------------------------ 模糊PID ---------------------------------------------//
/*论域*/
#define NB -6
#define NM -4
#define NS -2
#define ZO 0
#define PS 2
#define PM 4
#define PB 6
int8 ruleKp[7][7] = {
/* \ Ec */ /* NB 负大 NM 负中 NS 负小 ZO 零 PS 正小 PM 正中 PB 正大 */
/* E \ */ /* -6 -4 -2 0 2 4 6*/
/* NB 负大 -6 */ PB , PB , PM , PM , PS , ZO , ZO ,
/* NM 负中 -4 */ PB , PB , PM , PS , PS , ZO , NS ,
/* NS 负小 -2 */ PM , PM , PM , PS , ZO , NS , NS ,
/* ZO 零 0 */ PM , PM , PS , ZO , NS , NM , NM ,
/* PS 正小 2 */ PS , PS , ZO , NS , NM , NM , NM ,
/* PM 正中 4 */ PS , ZO , NS , NS , NM , NB , NB ,
/* PB 正大 6 */ ZO , ZO , NS , NM , NM , NB , NB ,
};
/*Ki规则表*/
int8 ruleKi[7][7] = {
/* \ Ec */ /* NB 负大 NM 负中 NS 负小 ZO 零 PS 正小 PM 正中 PB 正大 */
/* E \ */ /* -6 -4 -2 0 2 4 6*/
/* NB 负大 -6 */ NB , NB , NM , NM , NS , ZO , ZO ,
/* NM 负中 -4 */ NB , NB , NM , NM , NS , ZO , ZO ,
/* NS 负小 -2 */ NB , NM , NS , NS , ZO , PS , PS ,
/* ZO 零 0 */ NM , NM , NS , ZO , PS , PM , PM ,
/* PS 正小 2 */ NM , NS , ZO , PS , PS , PM , PM ,
/* PM 正中 4 */ ZO , ZO , PS , PS , PM , PB , PB ,
/* PB 正大 6 */ ZO , ZO , PS , PM , PM , PB , PB ,
};
/*Kd规则表*/
int8 ruleKd[7][7] = {
/* \ Ec */ /* NB 负大 NM 负中 NS 负小 ZO 零 PS 正小 PM 正中 PB 正大 */
/* E \ */ /* -6 -4 -2 0 2 4 6*/
/* NB 负大 -6 */ NB , NB , NM , NB , NM , ZO , ZO ,
/* NM 负中 -4 */ NB , NB , NM , NM , NS , ZO , PS ,
/* NS 负小 -2 */ NM , NM , NS , NS , ZO , PS , PS ,
/* ZO 零 0 */ NM , NM , NS , ZO , PS , PM , PM ,
/* PS 正小 2 */ NS , NS , ZO , PS , PS , PM , PM ,
/* PM 正中 4 */ NS , ZO , PS , PM , PM , PB , PB ,
/* PB 正大 6 */ ZO , ZO , PM , PB , PM , PB , PB ,
};
//statement
FuzzyPIDTypedef ServoFuzzyPID;
/*隶属度计算函数*/
void Calculate_Membership(float qv,int * index,float *ms)
{
if((qv>=NB)&&(qv<NM))
{
index[0]=0;
index[1]=1;
ms[0]=-0.5*qv-2.0; //y=-0.5x-2.0
ms[1]=0.5*qv+3.0; //y=0.5x+3.0
}
else if((qv>=NM)&&(qv<NS))
{
index[0]=1;
index[1]=2;
ms[0]=-0.5*qv-1.0; //y=-0.5x-1.0
ms[1]=0.5*qv+2.0; //y=0.5x+2.0
}
else if((qv>=NS)&&(qv<ZO))
{
index[0]=2;
index[1]=3;
ms[0]=-0.5*qv; //y=-0.5x
ms[1]=0.5*qv+1.0; //y=0.5x+1.0
}
else if((qv>=ZO)&&(qv<PS))
{
index[0]=3;
index[1]=4;
ms[0]=-0.5*qv+1.0; //y=-0.5x+1.0
ms[1]=0.5*qv; //y=0.5x
}
else if((qv>=PS)&&(qv<PM))
{
index[0]=4;
index[1]=5;
ms[0]=-0.5*qv+2.0; //y=-0.5x+2.0
ms[1]=0.5*qv-1.0; //y=0.5x-1.0
}
else if((qv>=PM)&&(qv<=PB))
{
index[0]=5;
index[1]=6;
ms[0]=-0.5*qv+3.0; //y=-0.5x+3.0
ms[1]=0.5*qv-2.0; //y=0.5x-2.0
}
}
//模糊PID实现
float FuzzyPID(float *pid_param, FuzzyPIDTypedef *pid,float _nowval,float _exval)
{
//读取数值
pid->kp1 = pid_param[FUZZYPID_Kp1];
pid->ki1 = pid_param[FUZZYPID_Ki1];
pid->kd1 = pid_param[FUZZYPID_Kd1];
pid->kp2 = pid_param[FUZZYPID_Kp2];
pid->ki2 = pid_param[FUZZYPID_Ki2];
pid->kd2 = pid_param[FUZZYPID_Kd2];
pid->sumError = pid_param[FUZZYPID_SumMax];
pid->exval = _exval;
pid->nowval = _nowval;
//更新当前误差
pid->nowError = pid->exval - pid->nowval;
pid->ErrorInc = pid->nowError - pid->lastError;
//误差以及误差增量限幅
pid->nowError = limit(pid->nowError,pid->nowErrmax);
pid->ErrorInc = limit(pid->ErrorInc,pid->ECmax);
TEST=myabs(pid->nowError);
if(myabs(MAX_ERROR)<=myabs(TEST))
{MAX_ERROR=myabs(TEST);}
/******** 模糊化 *******/
//量化 论域{-6，-5，-4，-3，-2，-1，0，1，2，3，4，5，6}
pid->E_Quantized = 6.0 * pid->nowError / pid->nowErrmax;
pid->EC_Quantized = 6.0 * pid->ErrorInc / pid->ECmax;
//计算隶属度
Calculate_Membership(pid->E_Quantized,pid->E_Index,pid->E_Membership);
Calculate_Membership(pid->EC_Quantized,pid->EC_Index,pid->EC_Membership);
/******** 解模糊 *******/
//计算kx2的系数
pid->kp2k =
pid->E_Membership[0] * pid->EC_Membership[0] * ruleKp[pid->E_Index[0]][pid->EC_Index[0]] +\
pid->E_Membership[0] * pid->EC_Membership[1] * ruleKp[pid->E_Index[0]][pid->EC_Index[1]] +\
pid->E_Membership[1] * pid->EC_Membership[0] * ruleKp[pid->E_Index[1]][pid->EC_Index[0]] +\
pid->E_Membership[1] * pid->EC_Membership[1] * ruleKp[pid->E_Index[1]][pid->EC_Index[1]] ;
pid->ki2k =
pid->E_Membership[0] * pid->EC_Membership[0] * ruleKi[pid->E_Index[0]][pid->EC_Index[0]] +\
pid->E_Membership[0] * pid->EC_Membership[1] * ruleKi[pid->E_Index[0]][pid->EC_Index[1]] +\
pid->E_Membership[1] * pid->EC_Membership[0] * ruleKi[pid->E_Index[1]][pid->EC_Index[0]] +\
pid->E_Membership[1] * pid->EC_Membership[1] * ruleKi[pid->E_Index[1]][pid->EC_Index[1]] ;
pid->kd2k =
pid->E_Membership[0] * pid->EC_Membership[0] * ruleKd[pid->E_Index[0]][pid->EC_Index[0]] +\
pid->E_Membership[0] * pid->EC_Membership[1] * ruleKd[pid->E_Index[0]][pid->EC_Index[1]] +\
pid->E_Membership[1] * pid->EC_Membership[0] * ruleKd[pid->E_Index[1]][pid->EC_Index[0]] +\
pid->E_Membership[1] * pid->EC_Membership[1] * ruleKd[pid->E_Index[1]][pid->EC_Index[1]] ;
//计算总的kx
pid->kp = pid->kp1 + pid->kp2 * (myabs(pid->kp2k) / 6.0f);//除以论域极值 计算的是kp增量百分比
pid->ki = pid->ki1 + pid->ki2 * (myabs(pid->ki2k) / 6.0f);
pid->kd = pid->kd1 + pid->kd2 * (myabs(pid->kd2k) / 6.0f);//除以论域极值 计算的是kp增量百分比
//计算输出
pid->out = \
pid->kp * (float)pid->nowError + \
pid->ki * (float)(pid->nowError + pid->lastError + pid->preError) + \
pid->kd * (float)(pid->nowError - 0.7 * pid->lastError - 0.3 * pid->preError);
//更新误差
pid->preError = pid->lastError;
pid->lastError = pid->nowError;
//输出限幅
pid->out = limit(pid->out,pid->outmax);
//输出
return pid->out;
}
舵机转向偏差计算
/**
*@Name :DirectionControl
*@Description :DirectionControl
*@Param :None
*@Return :None
*@Sample :DirectionControl();
**/
void DirectionControl(void)
{
if (CarInfo.ClosedLoopMode==DirectLoop || \
CarInfo.ClosedLoopMode==AllLoop || \
CarInfo.ClosedLoopMode==SpeedLoop) {
if (CarInfo.Protect_Flag=='F'){
DirectionOut = FuzzyPID(ServoFuzzyPIDParam[0],&ServoFuzzyPID, ELC_CenterRepair, 0);
// DirectionOut = Position_PID(ServoNonLinearPIDParam[0],&ServoNonLinearPID,0,ELC_CenterRepair);
}
else {
DirectionOut = 0;
}
ServoAdd = DirectionOut;
ServoWrite();
}
else return;
}电机差速控制
电机差速控制是指通过控制车辆两侧电机的转速差异来实现车辆的转向和转弯。一般来说，当车辆需要向左转时，右侧电机的转速会比左侧电机的转速快，从而使车辆向左转弯。反之，当车辆需要向右转时，左侧电机的转速会比右侧电机的转速快，从而使车辆向右转弯。
阿克曼系数是指车辆转向时前轮转角与后轮转角之间的比值。在智能车竞赛中，阿克曼系数的大小会影响车辆的转弯半径和稳定性。一般来说，阿克曼系数越大，车辆的转弯半径越小，但稳定性也会相应降低。反之，阿克曼系数越小，车辆的转弯半径越大，但稳定性也会相应提高。
/**
*@Name :DiffSpeedControl
*@Description :DiffSpeedControl差速控制
*@Param :None
*@Return :NULL
*@Sample :DiffSpeedControl();
**/
void DiffSpeedControl(void)
{
if (CarInfo.ClosedLoopMode == AllLoop && CarInfo.Protect_Flag == 'F') {
if(ServoAdd<=-70) {//右转
Speed_diff_l=ExSpeed;
Speed_diff_r=(int)(ExSpeed*(1+((float)Ackerman_k/1000)*tan((float)ServoAdd*3.14/3136)/0.396));
}
else if(ServoAdd>=70) {//左转
Speed_diff_l=(int)(ExSpeed*(1-((float)Ackerman_k/1000)*tan((float)ServoAdd*3.14/3136)/0.396));
Speed_diff_r=ExSpeed;
}
else {
Speed_diff_l=ExSpeed;
Speed_diff_r=ExSpeed;
}
}
else
{
ExSpeed_L=ExSpeed;
ExSpeed_R=ExSpeed;
}
}※转向控制代码
#include "control.h"
CarInfoTypedef CarInfo;
//定时器中断 1ms
uint16 NormalSpeed =320;
uint16 NormalSpeed1 =320;
uint16 NormalSpeed2 =320;
int16 ExSpeed=200;
int16 ExSpeedAdd=0;
int Ackerman_k =250;//左右差速系数
int Ackerman_k1 =250;
int Ackerman_k2 =230;
int16 ExSpeed_L = 0;
int16 ExSpeed_R = 0;
int16 SpeedOut_L = 0;
int16 SpeedOut_R = 0;
int16 DirectionOut = 0;
uint16 Outgargespeed=330;
uint16 IslandSpeed =320;
uint16 Fork1=260;
uint16 Fork2=320;
uint16 Fork3=320;
uint16 LFork1=250;
uint16 LFork2=250;
uint16 RampSpeed =160;
uint16 LongStraightSpeed=350;
uint16 Ingargespeed1=300;
uint16 Ingargespeed2=300;
uint16 Cnt;
int16 Coe=300;
int16 AddMax=15;
uint8 flag1 = 0;
uint8 StartCar = 1;
uint8 CarOutProtect='T';
//初始化一些设置
uint8 CarSystem_Init(void)
{
//设置标志
CarInfo.Protect_Flag = 'F';
//ret
return 1;
}
/**
*@Name :Timer6_IT
*@Description :Timer6_IT基本定时器6中断服务函数，用于电机控制及其他中断服务
*@Param :None
*@Return :NULL
*@Sample :Timer6_IT();
**/
void Timer_IT(void)
{
KeyScanInt();
if(!flag1){
Cnt++;
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();//电机控制
}
//------------------------------清零计数------------------------------------//
if(Cnt>= 1000) Cnt= 0;
}
}
void MoBan_Speed_Up(void)
{
if(
sqrt(myabs(LNor[L1]-LNor[L5]))+sqrt(myabs(LNor[L4]-LNor[L8]))+sqrt(myabs(LNor[L6]-LNor[L7]))<=10
)
{
ExSpeed=ExSpeed+100;
BeepTick(1, 100);
}
}
//舵机PID控制
void DirectionControl(void)
{
if(OutGarage.state == GarageOutTurn||OutGarage.state ==GarageIn\
||Island.state == IslandInTurn||Island.state ==IslandOutTurn \
||Fork.state == Fork_in||Fork.state == Fork_end\
||InGarage.state == GarageInTurn||InGarage.state == GarageStraighten||InGarage.state == GarageEnd\
||BackGarage.state == BackGarageForward||BackGarage.state == BackGarageTurn||BackGarage.state == BackGarageBackStraight){
ServoAdd=Forced;
}
else
{
if(Island.FindFlag == 'T'){
DirectionOut = FuzzyPID(ServoFuzzyPIDParam[1],&ServoFuzzyPID, 0, ELC_Center);
// DirectionOut = Position_PID(ServoNonLinearPIDParam[0],&ServoNonLinearPID,ELC_Center,0);
}
else if(Fork.FindFlag == 'T')
{
DirectionOut = FuzzyPID(ServoFuzzyPIDParam[2],&ServoFuzzyPID, 0, ELC_Center);
}
else
{
DirectionOut = FuzzyPID(ServoFuzzyPIDParam[0],&ServoFuzzyPID, 0, ELC_Center);
}
ServoAdd = DirectionOut;
}
/*********************************/
// if(Island.state == IslandOutTurn){
// ServoAdd= limit_ab(ServoAdd,620,720) ;
// }
// if(Island.state == IslandIn){
// ServoAdd= limit_ab(ServoAdd,620,720) ;
// }
// if(Cross.state==Crossin){
// ServoAdd= limit_ab(ServoAdd,0,800) ;
// }
// if(Island.state == IslandOutCenter)
// {
// ServoAdd= limit_ab(ServoAdd,-500,300) ;
// }
ServoWrite();
}
//差速控制
void DiffSpeedControl(void)
{
if (CarInfo.Protect_Flag == 'F'&&Island.FindFlag == 'F'&&Fork.FindFlag == 'F'){
if(ServoAdd<=-70) {//右转
ExSpeed_L=ExSpeed;
ExSpeed_R=(int)(ExSpeed*(1+((float)Ackerman_k/1000)*tan((float)ServoAdd*3.14/3136)/0.396));
}
else if(ServoAdd>=0) {//左转
ExSpeed_L=(int)(ExSpeed*(1-((float)Ackerman_k/1000)*tan((float)ServoAdd*3.14/3136)/0.396));
ExSpeed_R=ExSpeed;
}
else {
ExSpeed_L=ExSpeed;
ExSpeed_R=ExSpeed;
}
}
}
//速度PID控制
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();
}
//冲出赛道电磁削减保护
int16 CarOutProtectCnt = 0;
void Car_Protect(void)
{
//---------------电磁保护--------------//&&Fork.FindFlag == 'F'&&Island.FindFlag == 'F'
if(CarOutProtect=='T')
{
if(ELC_PROTECT_FLAG == 'T'&&OutGarage.Flag == 'T'&&BackGarage.FindFlag=='F')
{
CarOutProtectCnt ++;
}
else
{
CarOutProtectCnt=0;
}
if(CarOutProtectCnt >=50)
{
CarInfo.Protect_Flag = 'T';
}
}
}
