前言：上次云里霧里的說了一通，不知道對平衡車的控制有沒有說到點子上。單純的講解原理可能會很無聊，但是作為一個技術宅來說，就算頭皮發(fā)麻也要接著看下去。哈哈，吾理小子爭取用通俗的語言把自己懂的知識講解出來。

好了，閑話少說，進入正題。上文已經(jīng)做好了平衡車站立起來的全部準備工作，接下來就是控制的核心了，如果對上面講到的內(nèi)容還沒有看到，建議先看上一篇，否則會有莫名其妙的感覺。

首先，說說陀螺儀的安裝位置，建議有條件的話裝在電機軸的正上方，兩輪子中點處，這樣做的好處是角速度響應比較平均。但是，吾理小子的條件有限，陀螺儀裝在了右側輪子附近，安裝位置看下圖。（PS：陀螺儀一定要裝穩(wěn)）

安裝好陀螺儀之后，接下來開始寫程序。關于MPU6050的程序，吾理小子不想啰嗦，直接說比較關鍵的地方吧。相信用過STM32的小伙伴對原子哥不會陌生吧，小編也是原子哥的受益者，在這里順便感謝一下原子哥。

小編移植了原子哥的MPU6050程序，所以要說一下初始化問題。我們知道陀螺儀上電之后要初始化，然后要自校準。原子哥的程序是以上電的時候陀螺儀的姿態(tài)為參考，解算姿態(tài)。也就是說，每次測量的姿態(tài)都是以上電時刻為基準的。那么可能有人和小編一樣納悶，如果每次都是相對的參考平面，那豈不是要每次扶著平衡車上電初始化啊?？紤]到這個問題之后，小編嘗試尋找MPU6050的絕對參考平面。

原子哥初始化程序中有一個DMP初始化，打開看看

看到下面有一個自檢函數(shù)，Go to你會發(fā)現(xiàn)有這樣一段代碼：

敲黑板，畫重點啦。這就是獲取當前加速度計的值，然后將其設置為基礎值。各位看官明白什么意思了吧，就是自檢函數(shù)中獲取加速度計的值，將此時的值設置為基礎值，也就是參考平面啦！

啦啦啦，那我們把這個值設置為零就和初始位置無關了吧，所以將此時的加速度計的值置零就行了，修改之后如下：

至此，我們上電之后的參考平面就是水平面啦，是不是很簡單呢！

好了，接下來就是絕對無聊的代碼與思路了。首先，簡單描繪一下控制思路

各位請看上圖，雖然它很簡陋，但是基本能夠說明問題。讀取陀螺儀數(shù)據(jù)，通過PID控制器計算得到PWM的占空比輸出，控制電機將車架姿態(tài)調(diào)整，繼續(xù)讀取陀螺儀數(shù)據(jù)，如此反復，就完成了動態(tài)調(diào)節(jié)。

程序中要實現(xiàn)這樣的效果，一般會使用單片機內(nèi)部定時器，開啟定時器中斷，在固定的時間節(jié)點去讀取陀螺儀數(shù)據(jù)，然后進行PID計算，最后控制電機進行一次調(diào)節(jié)。小編也是這樣實現(xiàn)PID調(diào)節(jié)的，定時器10ms中斷一次。定時器的配置與中斷服務函數(shù)就不貼了，只著重描述直立環(huán)的PID代碼。 

void PID_Balance_Cal(Balance_PID * PID,float Angle,float Gyro)
{
	PID->Now  = Angle - 2;	
	PID->Out  = PID->Kp * PID->Now
	          + PID->Kd * Gyro;
}

這是直立環(huán)PID代碼，入口參數(shù)有三個。第一個是PID結構體，第二個是測量的角度值，第三個是角速度值。后兩個量與MPU6050安裝方位有關。小編的陀螺儀安裝位置與姿態(tài)第一幅圖拍過了，小車前傾時roll角度變小，后仰時roll角度變大。小車前傾與后仰時，陀螺儀的數(shù)據(jù)也就是角速度變化的是x軸的，也就是gyrox。因此，調(diào)用PID的形參就是roll和gyrox。

再來看函數(shù)具體內(nèi)容，第一行是計算當前誤差，2代表的是小編平衡車的機械中值，這個在上一篇的開頭中有說明。車架與重量分布不一樣，這個值也不一樣，一般都是在0°左右。第二行是PID的輸出，就是常規(guī)位置式PID的公式。這里我們只用到了PD項，沒有用到積分項，這是借鑒前人的經(jīng)驗而已。至于微分項為什么乘以gyro，現(xiàn)在進行一點自己的理解與說明。

大家調(diào)試過位置式PID的同志們都知道，計算公式是

PID->Out=(PID->Kp * PID->Error) + (PID->Ki * PID->Error_I) + (PID->Kd * PID->Error_D); 

在平衡車中用到的PID形式好像有點看不懂了。有沒有這種感覺，其實并不是這樣的，上面兩個實質(zhì)是一樣的。一般來說，求微分項就是求角度的變化趨勢，也就是求角度的微分，他的物理意義實際就是角速度，因此我們直接用陀螺儀角速度乘以微分項系數(shù)來表示微分結果，這是合情合理的吧！各位道友，不知小編的理解是否正確，歡迎各位批評指正。

好了，直立環(huán)的核心代碼就這么幾行，接下來就是參數(shù)整定了。

嗯……再三考慮，還是先把定時器中斷里面的代碼貼一下吧！

typedef struct
{
	float Kp;
	float Ki;
	float Kd;
	float Out;
	float Now;
	float Last;
	float Earlier;
	float Error;
	float Error_I;
	float Error_D;
} Balance_PID;  //定義直立環(huán)結構體，很多參數(shù)都是冗余。小編太懶，不想改
Balance_PID  Balance_pid; //初始化直立環(huán)結構體
void TIM5_IRQHandler(void)
{
    if (TIM_GetITStatus(TIM5, TIM_IT_Update) != RESET) 
	{
	    TIM_ClearITPendingBit(TIM5, TIM_IT_Update); //清中斷標志
	    if(mpu_dmp_get_data(&pitch,&roll,&yaw)==0)  //讀取陀螺儀數(shù)據(jù)
	    { 
		    MPU_Get_Accelerometer(&aacx,&aacy,&aacz);	//獲取加速度計數(shù)據(jù)
		    MPU_Get_Gyroscope(&gyrox,&gyroy,&gyroz);	//獲取陀螺儀數(shù)據(jù)
		    PID_Balance_Cal(&Balance_pid,roll,gyrox);	//調(diào)用直立環(huán) 
		    Set_Motor_PWM((int)Balance_pid.Out,(int)Balance_pid.Out);  //設置PWM
	    }
	}
}

代碼應該沒有什么說的，自行看注釋就好。

先確定參數(shù)正負，再確定大小。

設置Kp=100。燒代碼，拿起小車，向前傾，發(fā)現(xiàn)車輪向后加速。這樣的效果也就是說輪子的響應是加速小車倒下，在不是我們想要的結果，肯定是正負錯了。

設置Kp=-100。燒代碼，拿起小車，向前傾，車輪向前加速，向后傾，車輪向后加速，如此這就是正常的效果了。

同樣的實驗過程確定Kd：

設置Kp=0，Kd=0.5。燒代碼，拿起小車，猛向前傾，發(fā)現(xiàn)車輪向后加速。這樣的效果也就是說正負錯了。

設置Kp=0，Kd=-0.5。燒代碼，拿起小車，猛向前傾，車輪向前加速，猛向后傾，車輪向后加速，正常的效果。

上面的參數(shù)大小為什么這樣取值，和PWM的計數(shù)上限有關。參數(shù)大小范圍，可以這樣評估。假如計數(shù)器上限是2000，車架的角度變化是-9°~9°，那么我們的Kp大概就在（2000/18=）100左右。我們知道P的系數(shù)是調(diào)整的步幅，步子太大，響應速度快，但是不穩(wěn)定，步子太小可能達不到期望值?；揪褪沁@個意思了。上面設置d的系數(shù)很小，是因為小車猛向前或者后時，gyrox的值可能達到上萬的數(shù)值，所以這個只取0.5。好了，具體的細節(jié)就不多說了，參數(shù)整定多了，自然會明白。

至此，直立環(huán)就剩下參數(shù)的整定啦。各位可參見平衡小車之家的整定教程。講道理，參數(shù)整定的好的話，小車應該可以很聽話的站立起來了。但是小編遇見了下面的問題：

基本現(xiàn)象是小車能夠站立，也能夠看出在不斷調(diào)整，但是小車總會慢慢旋轉，就是右輪的動作幅度很大，左輪幾乎不動，看到的現(xiàn)象就是小車在慢慢的原地轉圈。其實小車穩(wěn)定性還是很高的，但是就是會莫名的轉圈，再怎樣調(diào)整參數(shù)，都無法解決這個問題。小編有點抓狂了，好難受。（抱住肉肉的自己）

隨后，仔細回顧代碼發(fā)現(xiàn)，PD計算出的PWM控制兩個電機是相同的值。靈光一閃，我已經(jīng)知道問題在哪里啦，肯定是驅(qū)動板不對稱或者電機參數(shù)不對稱造成的，也就是說給相同的PWM兩個電機響應的不一樣。我決定嘗試用編碼器來測試一下，看來土豪級的編碼器還是排上用場了。具體這樣操作的：

初始化左右兩電機的編碼器，設置PWM：Set_Motor_PWM(1200,1200);

void Get_Speed_Data(void)          //獲取當前速度
{
	Left_encode_val = TIM_GetCounter(TIM8);
	Right_encode_val= TIM_GetCounter(TIM4);	//讀計數(shù)器值
	TIM8->CNT = 15000;    //將計數(shù)值賦初值
	TIM4->CNT = 15000;
	Left_Speed_Val= (15000-Left_encode_val)*2.578;  //uint:m/s    *1.0035
	Right_Speed_Val=(15000-Right_encode_val)*2.578;  //uint:m/s //計算實際速度
}

編寫讀取編碼器數(shù)據(jù)子函數(shù)，為了放置縮小誤差，我們直接看編碼器的值。在定時器中斷中調(diào)用上面子函數(shù)，然后把得到的值實時顯示在SPI接口屏幕上。（豪華配置，一個個都體現(xiàn)了出來）如果沒有屏幕，此時可以用串口看數(shù)據(jù)。在死循環(huán)中調(diào)用顯示函數(shù)，具體代碼

	while(1)
	{
//		sprintf(s,"%.3f",roll);
//		Gui_DrawFont_GBK16(60,16,RED,GRAY0,(unsigned char*)s);
		sprintf(s,"%d",Left_encode_val-15000);
		Gui_DrawFont_GBK16(60,80,RED,GRAY0,(unsigned char*)s);
		sprintf(s,"%d",Right_encode_val-15000);
		Gui_DrawFont_GBK16(60,96,RED,GRAY0,(unsigned char*)s);
	} 

將左右輪的編碼器值顯示在屏幕上。如此，在PWM均為1200情況下，在10ms（定時器中斷間隔）時間段內(nèi)，左右電機編碼器的值，左輪數(shù)值22左右，右輪數(shù)值34左右。

我的天吶！各位發(fā)現(xiàn)了沒有，10ms的時間居然相差這么大，那么為什么會原地轉動，原因和預料的一樣。

怎么辦嘞？補償唄，通過補償發(fā)現(xiàn)，當設置左輪為1370時，和右輪編碼器數(shù)值幾乎相等，這就圓滿啦（哈哈哈，興奮一下）。注意，反相同樣需要補償。

補償之后，還原原來的代碼，燒代碼，看效果。

哇，見證奇跡的時刻到了，小車直立環(huán)效果感人?。≌玖⒌?非常穩(wěn)，速度幾乎為零。

各位道友可能看到說僅一個直立環(huán)是不足以讓小車站穩(wěn)的，那么小編的結論并非如此，僅僅直立環(huán)也可以站穩(wěn)而且很穩(wěn)。

PS：僅直立環(huán)是否能夠站穩(wěn)和小車的安裝、結構、重量分布等等都有關系，因此，調(diào)試過程中一定以自己的現(xiàn)象為準，切記不能盲從。

最關鍵的直立環(huán)調(diào)試完了，效果感人，那么今天的內(nèi)容也接近尾聲啦！

隨后的速度與轉向環(huán)，吾理小子不想再接著寫了，因為沒有特別難的，各位自己修行吧，哈哈哈！多說一句，速度環(huán)不是常規(guī)的負反饋，而是正反饋。這樣描述一下，就是小車速度要想降下來，必須先加速然后才能減速。欲知具體效果，需自身體驗，謝謝！

感謝原子哥和平衡小車之家，有了你們這些大佬的默默付出，才會有我們這群小妖怪的茁壯成長！

最后，寫的有點凌亂，有什么不對的地方請各位批評指正！