智能车入门补充篇——电感值处理、转向控制与巡线

news2024/10/7 6:45:38

冒泡法

冒泡法是一种简单的排序算法，它重复地遍历要排序的数列，一次比较两个元素，如果它们的顺序错误就把它们交换过来。遍历数列的工作是重复地进行直到没有再需要交换，也就是说该数列已经排序完成。冒泡排序的时间复杂度为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;

滤波

滤波是一种信号处理方法，用于去除信号中的噪声或不需要的成分。常见的滤波方法包括低通滤波、高通滤波、带通滤波和带阻滤波等。滤波的实现可以使用数字滤波器或模拟滤波器。

卡尔曼滤波是一种用于估计系统状态的滤波方法，它可以通过对系统的测量值和模型进行融合来提高状态估计的精度。卡尔曼滤波的核心思想是通过对系统状态的预测和测量值的校正来不断更新状态估计值。卡尔曼滤波广泛应用于导航、控制、信号处理等领域。

差比和加权算法

差比和

  1. 读取左右两个电磁感应模块的电磁信号值；
  2. 对左右两个电磁感应模块的电磁信号值进行差分处理，得到一个差值；
  3. 根据差值的正负来判断小车是否偏离了中线；
  4. 根据差值的大小来调整小车的转向角度，使其重新回到中线上。

加权

加权算法是一种常用的循迹算法，它通过对左右两个电磁感应模块的电磁信号进行加权处理，得到一个加权值，用来判断小车是否偏离了中线。根据加权值的大小来调整小车的转向角度，使其重新回到中线上。

直接调节电感差比和差加权算法可能会因无法直观的了解各参数而走弯路，建议先调出一套相对稳定的电感差比和加权方案，然后将其改写为电感差比和差加权算法。

使用差比和差加权算法时，小车在弯道道型出现小幅度过调，此时轻微降低比例系数即可完全拟合。

	//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';
	}
  }
}

本文来自互联网用户投稿，该文观点仅代表作者本人，不代表本站立场。本站仅提供信息存储空间服务，不拥有所有权，不承担相关法律责任。如若转载，请注明出处：http://www.coloradmin.cn/o/1234266.html

如若内容造成侵权/违法违规/事实不符，请联系多彩编程网进行投诉反馈，一经查实，立即删除！

相关文章

电脑上可以写便签的软件哪些界面比较可爱且好用?

电脑上可以写便签的软件哪些界面比较可爱且好用?

电脑上可以安装使用的便签类软件比较多&#xff0c;在选择使用电脑便签软件时&#xff0c;很多人对便签的外观界面还是比较在意的&#xff0c;一个好看的便签界面在一方面可以引起大家的注意&#xff0c;另一方面可以增加电脑桌面背景和便签类软件的协调性。 电脑便签软件通常…
阅读更多...
Web测试中文件上传测试

Web测试中文件上传测试

总体情况 1、功能实现&#xff1a; &#xff08;1&#xff09;文件类型正确、大小合适 &#xff08;2&#xff09;文件类型正确&#xff0c;大小不合适 &#xff08;3&#xff09;文件类型错误&#xff0c;大小合适 &#xff08;4&#xff09;文件类型和大小都合适&#x…
阅读更多...
引迈-JNPF低代码项目技术栈介绍

引迈-JNPF低代码项目技术栈介绍

从 2014 开始研发低代码前端渲染&#xff0c;到 2018 年开始研发后端低代码数据模型&#xff0c;发布了JNPF开发平台。 谨以此文针对 JNPF-JAVA-Cloud微服务 进行相关技术栈展示&#xff1a; 1. 项目前后端分离 前端采用Vue.js&#xff0c;这是一种流行的前端JavaScript框架&a…
阅读更多...
风丘远程试验管理平台TFM 支持“一键式“数据管理和监控

风丘远程试验管理平台TFM 支持“一键式“数据管理和监控

随着信息技术的高速发展&#xff0c;企业对远程试验实时监控与数据管理的需求日益增强。而利用远程试验信息协同技术&#xff0c;可突破部门与地域的限制&#xff0c;并把试验现场的车辆状态信息、试验数据和分析结果实时传输给数据分析部门和设计部门等&#xff0c;从而缩短时…
阅读更多...
SQL的连接join

SQL的连接join

一、连接说明 union、intersect等集合运算&#xff0c;它的特征是以 “行” 为单位进行操作&#xff0c;通俗点说&#xff0c;就是进行这些集合运算&#xff0c;会导致记录行数的增减&#xff0c;使用union会增加记录行数&#xff0c;使用 intersect 或 expect 会减少行记录&a…
阅读更多...
印刷企业数字工厂管理系统建设方案

印刷企业数字工厂管理系统建设方案

随着科技的飞速发展&#xff0c;传统印刷企业正面临着前所未有的挑战。产能短缺、质量控制不足、成本控制不力以及服务质量不高等问题成为了制约企业发展的瓶颈。为了解决这些问题&#xff0c;印刷企业需要积极拥抱数字化转型&#xff0c;构建数字工厂管理系统。 一、印刷企业数…
阅读更多...
Figma 是什么软件？为什么能被Adobe收购

Figma 是什么软件？为什么能被Adobe收购

很多人一定早就听说过Figma的名字了。看到很多设计同行推荐&#xff0c;用了很久&#xff0c;疯狂的安利朋友用。是什么让这么多设计师放弃了FigmaSketch的魅力&#xff1f;下面的内容将详细分享一些与Figma相关的知识点&#xff0c;并介绍这个经常听到但不熟悉的工具。 Figma…
阅读更多...
nacos鉴权报invalid username or password

nacos鉴权报invalid username or password

操作 你得检查一下nacos的配置的数据库有没有缺少表&#xff0c;可以在下图找到nacos的官方的配置库&#xff1a; 然后注意到这个SQL文件的最后的两行&#xff0c;这两行就是插入默认的nacos的登录密码的&#xff0c;如果你设置了对应的配置的文件其实也是没有用的最后他还是…
阅读更多...
最小最大表示法超详细讲解

最小最大表示法超详细讲解

文章目录 前言循环同构串最小表示法暴力解法暴力解法的可优化之处最小表示法的代码实现 最大表示法 前言 最小/最大表示法是用来找出字符串的最小/最大字典序的循环同构串的方法&#xff0c;其求解算法可以达到O(N)&#xff0c;过程很像KMP算法的next数组推导过程&#xff0c;都…
阅读更多...
k8s-pod管理 3

k8s-pod管理 3

pod是可以创建和管理k8s 计算的最小可部署单元&#xff0c;一个pod 代表着集群中运行的一个进程&#xff0c;每个pod 都有一个唯一的ip pod包裹了容器 下载测试镜像 创建自主式的pod 查看创建的pod的详情信息 删除pod 创建控制器 副本过多&#xff0c;需要进行负载均衡减轻节点…
阅读更多...
uniapp中实现圆形进度条的方式有哪些？

uniapp中实现圆形进度条的方式有哪些？

前言 在uniapp开发小程序或者apk时&#xff0c;页面需要用到一个圆形进度条&#xff08;带文字和百分比的&#xff09;&#xff0c;自己也自定义过一个,但是有一点小问题&#xff0c;咱先展示如何引入插件市场的在介绍自定义的&#xff01;一共四种&#xff0c;但是你需要考虑自…
阅读更多...
G管螺纹尺寸对照表

G管螺纹尺寸对照表

G管螺纹尺寸对照表 NPT 是 National (American) Pipe Thread 的缩写&#xff0c;属于美国标准的 60 度锥管螺纹&#xff0c;用于北美地区&#xff0e;国家标准可查阅 GB/T12716-1991 PT 是 Pipe Thread 的缩写&#xff0c;是 55 度密封圆锥管螺纹&#xff0c;属惠氏螺纹家族&a…
阅读更多...
视频剪辑技巧：如何高效地将多个视频合并成一个新视频

视频剪辑技巧：如何高效地将多个视频合并成一个新视频

在视频制作过程中&#xff0c;将多个视频合并成一个新视频是一个常见的操作。这涉及到将多个片段组合在一起&#xff0c;或者将不同的视频素材进行混剪。无论是制作一部完整的影片&#xff0c;还是为社交媒体提供短视频&#xff0c;都要掌握如何高效地将多个视频合并。现在一起…
阅读更多...
广州华锐互动：3D模拟体验火箭发射过程有什么教育意义？

广州华锐互动：3D模拟体验火箭发射过程有什么教育意义？

3D虚拟还原火箭发射过程在教育领域有着深远的意义。它能够为学生提供直观、生动的体验&#xff0c;帮助他们更好地理解和掌握相关知识&#xff0c;激发他们对科技的兴趣和探索欲望。 首先&#xff0c;3D虚拟还原火箭发射过程可以帮助学生更好地理解和掌握航天科技知识。在火箭发…
阅读更多...
vue-waterfall2 实现瀑布流，及总结的问题

vue-waterfall2 实现瀑布流，及总结的问题

注意&#xff1a;引入需要在主界面引入&#xff0c;直接在组件中引用会有问题 1.安装 npm install vue-waterfall21.8.20 --save &#xff08;提示&#xff1a;一定要安装1.8.20&#xff0c;最新版会有一部分问题&#xff09; 2.打开main.js文件 import waterfall from v…
阅读更多...
python 实现等声值线图绘制

python 实现等声值线图绘制

今天讲一类环评项目的噪声预测 - 风电 风机噪声作为面源目前难有成熟的模型进行预测。根据国内外的研究&#xff0c;都是根据与风机中心的位置进行分级预测。 翟国庆等利用美国航天航空局&#xff08;NASA&#xff09;研发的风电机组噪声预测模型&#xff08;以下简称 NASA”…
阅读更多...
数字化转型导师坚鹏：数字化时代银行网点厅堂营销5大重点分析

数字化转型导师坚鹏：数字化时代银行网点厅堂营销5大重点分析

数字化时代银行网点厅堂营销存在以下5大重点&#xff1a; 1、厅堂宣传。应以主推产品作为厅堂宣传的宣传重点&#xff0c;结合视频宣传、平面物料、互动机具、陈列物料等多维度&#xff0c;开展有序重复展示&#xff0c;进而加大吸引客户关注度。 2、产品推荐。在识别出中高端…
阅读更多...
基于混沌博弈算法优化概率神经网络PNN的分类预测 - 附代码

基于混沌博弈算法优化概率神经网络PNN的分类预测 - 附代码

基于混沌博弈算法优化概率神经网络PNN的分类预测 - 附代码 文章目录 基于混沌博弈算法优化概率神经网络PNN的分类预测 - 附代码1.PNN网络概述2.变压器故障诊街系统相关背景2.1 模型建立 3.基于混沌博弈优化的PNN网络5.测试结果6.参考文献7.Matlab代码 摘要&#xff1a;针对PNN神…
阅读更多...
亚马逊美国站买家号注册流程

亚马逊美国站买家号注册流程

注册亚马逊美国站买家号一般用邮箱及手机号注册就可以了&#xff0c;具体操作如下&#xff1a; 1、在浏览器里面输入亚马逊美国站的官网地址。 2、点击注册&#xff0c;输入姓名、邮箱或手机号、密码&#xff0c;然后进行验证邮箱或者手机号。如果是用的邮箱进行注册验证&…
阅读更多...
MyBatis Generator 插件 详解自动生成代码

MyBatis Generator 插件 详解自动生成代码

MyBatis Generator&#xff08;MBG&#xff09;是MyBatis和iBATIS的代码生成器。可以生成简单CRUD操作的XML配置文件、Mapper文件(DAO接口)、实体类。实际开发中能够有效减少程序员的工作量&#xff0c;甚至不用程序员手动写sql。 它将为所有版本的MyBatis以及版本2.2.0之后的i…
阅读更多...
推荐文章
最新文章

Copyright @ 2022~2023