前言

今天心血來潮，突然想總結一下電磁車的一些基本演算法，也希望可以給新手同學們一點幫助和參考，
電磁車的控制相對比較簡單，大致可以分為信號采集，舵機控制和電機控制三部分，所謂電磁車控制演算法，其實就是對這三個模塊功能的研究和精確，從而使賽車達到我們想要的效果，

采樣信號濾波

什么是濾波

濾波（Wave filtering）是將信號中特定波段頻率濾除的操作，是抑制和防止干擾的一項重要措施，它可以從含有干擾的接收信號中提取有用信號，很大程度上保證了采集到的信號的真實性和穩定性，正因如此，濾波演算法也成了在智能車控制中不可或缺的一種控制演算法，

常見的軟體濾波辦法

軟體濾波在嵌入式的資料采集和處理中有著很重要的作用，對于電磁車來說，我們需要對采集到的賽道電磁信號進行濾波處理，這里我們介紹幾種常見的軟體濾波處理辦法，

限幅法

一般來說，每個電感在賽道上采集到的值都有一個固定的范圍，由于電感排布方案的不同（水平電感、豎直電感或斜電感等）或不同賽道電磁信號的差別，這個范圍也會隨之變化，這時我們可以在程式的初始化中加入一個掃描賽道的程式，在初始化程式運行時令車左右掃描，將每個電感的最大值和最小值分別記錄入兩個一維陣列中，讓其作為每個電感采集信號值的上下限（這樣也是為了提高賽車對于不同賽道的適應性），在后續賽車行駛程序中，便可以過濾在這個范圍之外的電感值，從而排除偶然誤差，達到軟體濾波的效果，
具體代碼如下（此處只寫一個其中電感的處理）：

// 掃描賽道 //
void saomiao()
{   
     uint32 i;
     uint16 max1=0,min1=4095;
        
     for(i=0;i<1000;i++)  
     {       
       LeftADC[0]=ADC_Ave(ADC0,ADC1_SE8,ADC_12bit,10); 
       if(LeftADC[0]>max1) 
	   	 max1=LeftADC[0];
       if(LeftADC[0]<min1)
      	 min1=LeftADC[0];
     }
     
} 


// 限幅法濾波 //
void AD_value()
{    
    LeftADC[0]=ADC_Ave(ADC0,ADC1_SE8,ADC_12bit,10); 
    if(LeftADC[0]>max1) 
	  	LeftADC[0]=max1;
    if(LeftADC[0]<min1)
        LeftADC[0]=min1;
        
} 

算術平均值濾波

算術平均值濾波，指的是將每個電感采集到的一組值用冒泡排序按從大到小或者從小到大的順序排布，再丟棄最大值與最小值，取剩下資料的算術平均數，作為反饋給單片機的實時資料，這也是最常見的軟體濾波演算法之一，具體代碼如下：

void Ad_Value()     
{  
   uint16  LeftADC[7];
   int i,j,t;
   for(i=0;i<7;i++)
    {   
    	LeftADC[i]= ADC_Ave(ADC0,ADC1_SE8 ,ADC_12bit,10); 
    }
    
    for(i=0;j<7;i++)    
    {
      if(LeftADC[j]>LeftADC[j+1])
      {
        t=LeftADC[j+1];
        LeftADC[j+1]=LeftADC[j];
        LeftADC[j]=t;
      }
    }
    
    LeftAverage[0]=(uint16)((LeftADC[1]+LeftADC[2]+LeftADC[3]+LeftADC[4]+LeftADC[5])/5.0);   
    
}

歸一化

什么是歸一化

歸一化就是將所有資料都變成0-1之間的數，將資料映射到0～1范圍之內處理，使資料觀察更便捷快速，在電磁車行駛程序中，由于需要通過分析各個電感采集值的情況來判定前方為何種路段，所以更要求這些采集值有跡可循，歸一化便是一種很好的方法，先將所有采集到的值縮小到0-1范圍內，再適當放大（常見的放大倍數為100），這樣既能更容易的由電磁值分析出路況，又不會因為資料過小而失真，

歸一化的代碼實作

歸一化的公式如下：(x-Min)/（Max-Min），
其中，x為實時檢測到的變數，Min與Max為標定的電感采集最小與最大值，
具體代碼如下：

    AD_M_Left[0] =(uint16)(99*(LeftAverage[0]-M_Left_min)/(M_Left_max[0]-M_Left_min)+1);

PID控制

在介紹PID演算法前，我們可以試想一下，如果沒有某些特殊的演算法，那么我們會如何控制舵機打角和電機轉速？就舵機而言，其結論可能是，設定一個特定的值，當左右電感的電磁值之差達到這個設定的值時，便控制舵機向左或向右打一定的角度（或者多設值，分不同情況多段打角），實際上這也是我在剛接觸智能車時用的演算法，這種演算法雖然能夠讓賽車在賽道上行駛，但打角不夠順滑，反應也不夠靈敏，這時我們便需要將這個差值代入某個演算法，令其與舵機打角關聯起來，這樣便可以得到一個較為連貫與精確的舵機輸出值，有利于賽車更完美的運行，而這種將差值與最后輸出值關聯起來的演算法便是我們常說的PID控制演算法，

什么是PID控制

比例（P）積分（I）微分（D）控制（PID控制），是應用最為廣泛的一種自動控制器，它具有原理簡單，易于實作，適用面廣，控制引數相互獨立，引數的選定比較簡單等優點，PID控制分為很多種，而智能車中常用增量式和位置式兩種演算法對車身進行控制，以保證賽車在賽道上完美運行，

PID演算法的代碼實作

舵機控制

在對舵機的控制中，我采用的是位置式PD演算法，將左右電感的偏差值賦給error，再代入公式進行解算，最后傳給舵機輸出子函式，控制舵機正確打角，

void steer_control()  
{
    error=AD_M_Left[0]-AD_M_Right[0];
    
    direction_controlout=dirP*error+dirD*(error-lasterror);
    angle=mid_angle+direction_controlout;
    
    angle=angle>right_angle?right_angle:angle;
    angle=angle<left_angle?left_angle:angle;
    
    PWM_SetSteer((int)angle); 
    lasterror=error;
    
}

另外，如果想使舵機更順滑，可以對偏差error的計算進行處理，如將歸一化后的值開方處理等，

電機控制

在對電機的控制中，我采用的是增量式PID演算法，首先要設定一個目標速度set_speed，再將當前速度與其比較，得出的差值代入公式計算，得到最終需要輸出的電機占空比，從而控制電機轉速，需要注意的是，電機PID控制作為倍訓控制，最好將其放入PIT定時器中斷，同時在中斷中用左右兩個編碼器分別測出實時速度，這樣可以使其控制的更精確，

void PID_computer_newL(int left_speed_in)
{
    ec_left=set_speed-left_speed_in;
    
    pwm_L+=speedL_P*(ec_left)+speedL_I*(ec_left-eb_left)+speedL_D*(ec_left-2*eb_left+ea_left);
    if(pwm_L> 600)  pwm_L=600;  
    if(pwm_L<-600)  pwm_L=-600; 
    
    PWM_SetMotorL((int)pwm_L);  
    
    ea_left=eb_left;
    eb_left=ec_left;
    
}

目標速度的確定

上面我們說了PID演算法可以依據反饋的差值，與最后想要的變數關聯起來，達到動態控制的效果，那么為什么不能設定同一個目標速度，讓賽車以同一速度跑完全程？為什么還要設定不同的目標速度？我們知道由于賽車自身機械性能的限制，在不同賽道元素中速度的上限不同，為了使賽車可以更快地跑完全程，我們需要根據賽道元素的不同設定更適合當前路段的目標速度，而對于電磁車來說，可以感知賽道的只有電磁傳感器，所以我們只能將左右電感的差值關聯目標速度，在差值小的時候認為賽車處于直道，此時設定一個最大速度，再通過PID演算法將電機轉速保持在這個水準；若差值偏大，則調小目標速度，從而達到控制目標速度動態變化的目的，
而在除錯程序中我們可能會發現，僅僅靠舵機打角并不能很好的使賽車精確轉彎，這時候我們就需要對兩個電機的目標速度進行分別控制，通過左右電機的差速輔助賽車轉彎，