今天心血來潮，突然想總結一下電磁車的一些基本演演算法，也希望可以給新手同學們一點幫助和參考。
電磁車的控制相對比較簡單，大致可以分為訊號採集，舵機控制和電機控制三部分，所謂電磁車控制演演算法，其實就是對這三個模組功能的研究和精確，從而使賽車達到我們想要的效果。
濾波（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控制演演算法。
比例（P）積分（I）微分（D）控制（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演演算法將電機轉速保持在這個水準；若差值偏大，則調小目標速度，從而達到控制目標速度動態變化的目的。
而在偵錯過程中我們可能會發現，僅僅靠舵機打角並不能很好的使賽車精確轉彎，這時候我們就需要對兩個電機的目標速度進行分別控制，通過左右電機的差速輔助賽車轉彎。