09_綜合實(shí)踐----兩輪機(jī)器人的平衡控制0515

1、第9章綜合實(shí)踐一兩輪機(jī)器人的平衡控制學(xué)習(xí)目標(biāo) 學(xué)習(xí)兩輪機(jī)器人的平衡方法 學(xué)習(xí)數(shù)字PID控制方法 學(xué)習(xí)在Eclipse下開發(fā)MultiFLEX?2-AVR控制器程序9.19.1兩輪平衡機(jī)器人1 1 . .任務(wù)背景兩輪自平衡小車類似于一階倒立擺， 由于其不穩(wěn)定的動態(tài)特性， 兩輪自平衡小車成為驗(yàn) 證各種控制算法的理想平臺，具有重要的理論意義。到現(xiàn)在為止，全世界范圍內(nèi)出現(xiàn)了非常 多的兩輪自平衡小車的成功例子，有些已經(jīng)運(yùn)用到現(xiàn)實(shí)生活領(lǐng)域，Segway代步車是最成功的代表。圖9.1 Segway代步車2 2. .任務(wù)要求本節(jié)的任務(wù)是將使用“創(chuàng)意之星套件設(shè)計并搭建一個類似于Segway的雙輪機(jī)器人,使其能夠

2、只使用兩個輪子便能保持動態(tài)平衡！9.29.2任務(wù)分析與規(guī)劃1 1 . .了解segwaysegwaySegway是如何實(shí)現(xiàn)自平衡的呢？我們先來分析它的原理，然后再設(shè)計自己的Segway。Segway的姿態(tài)控制思路是：檢測車體的傾斜角度，車體轉(zhuǎn)動角速度，車輛的行進(jìn)速度 等信息，通過調(diào)節(jié)輪子的轉(zhuǎn)動，提供回轉(zhuǎn)力矩抵消傾斜的趨勢，讓車體保持平衡。Segway主要由三個局部構(gòu)成：傳感器系統(tǒng)Segway使用了5個陀螺儀、2個傾斜傳感器用于檢測車體的姿態(tài)，配備磁性碼盤用于檢測電機(jī)輸出速度，還有其它傳感器用于檢測是不是有人站在車輛上等。控制系統(tǒng)控制系統(tǒng)通過處理各種傳感器的信號，計算車體當(dāng)前的姿態(tài)、 速度等信息

3、，然后控制電機(jī)的輸出速度和扭矩，讓車體保持平衡的前提下，能夠按照操縱者的意圖前進(jìn)、后退或者轉(zhuǎn) 彎。推進(jìn)系統(tǒng)推進(jìn)系統(tǒng)包含離合器、變速箱、車輪、電機(jī)等。這個系統(tǒng)將控制系統(tǒng)的電機(jī)控制指令轉(zhuǎn) 化成實(shí)際的輪子轉(zhuǎn)動輸出。2 2. .我們自己的方案Segway的系統(tǒng)是很龐大的，如果照搬它的設(shè)計方案，我們可能需要幾十個工程師才能 設(shè)計完本錢章的工程。 我們需要搭建的系統(tǒng)就是一個能夠自動平衡的機(jī)器人，并不需要能夠載人，segway的系統(tǒng)架構(gòu)對我們而言有參考價值，但方案的細(xì)節(jié)我們需要重新考慮。傳感器系統(tǒng)我們需要測量機(jī)器人的姿態(tài)，但不需要像segway一樣測量那么多的姿態(tài)信息，我們只需要獲取機(jī)器人前后俯仰的角度信息

4、即可。獲取機(jī)器人的前后傾斜角度后，可以通過控制電機(jī)的轉(zhuǎn)向，反向補(bǔ)償這個傾斜角，合理的補(bǔ)償方式就能夠讓機(jī)器人保持動態(tài)平衡。使用什么傳感器呢？開關(guān)量傳感器肯定是不行的，對于Segway這樣要求比擬高控制頻率和控制精度的系統(tǒng)而言，開關(guān)量傳感器的響應(yīng)速度、精度都不滿足要求。我們需要測量機(jī)器人的傾斜角度，如果直接能夠得到這個角度是最好的，如果不能，我們可以通過間接的手段來獲得。比方對角速度進(jìn)行積分，比方測量機(jī)器人前前方向兩個點(diǎn)對地面的距離，通過幾何關(guān)系也可以算出傾斜角度。如圖9.2 ,我們用兩個紅外測距傳感器來測量機(jī)器人前前方向兩個點(diǎn)對地面的距離，從 而間接得到機(jī)器人的傾斜角度。紅外測距傳感器可以得到a

5、、b的值，c的值確定，進(jìn)行幾何求解就可以得到角度。圖9.2角度測量示意圖控制系統(tǒng)和推進(jìn)系統(tǒng)控制系統(tǒng)我們選擇MultiFLEX?2-AVR控制器。相對于MultiFLEX?2-PXA27Q MultiFLEX?2-AVR控制器的編程更簡單，更容易實(shí)現(xiàn)segway要求的對傳感器的高速采集和高速響應(yīng)。CDS55xx舵機(jī)既可以用做舵機(jī)模式，也可以用做電機(jī)模式，我們這里用其作為segway的推進(jìn)系統(tǒng)。 機(jī)器人結(jié)構(gòu)我們需要搭建一個雙輪機(jī)器人，需要有兩個懸臂用于安裝紅外測距傳感器進(jìn)行機(jī)器人傾 斜角度的測量?？刂品椒ㄕ麄€結(jié)構(gòu)方案、傳感器方案都確定好了，我們可以得到機(jī)器人的傾斜角度，能夠有效控制機(jī)器人輪子的轉(zhuǎn)動

6、了，但是如何讓機(jī)器人保持平衡呢？如圖9.3所示，a-b= h,如果h0說明機(jī)器人向右傾斜，這時我們需要給一個順時 針的輪子轉(zhuǎn)動 3 設(shè)定順時針30,如果h0o因此我們可以得到一個控制關(guān)系式：3 =k* h,k是一個比例因子。我們只需要調(diào)整控制的周期和k的參數(shù)就能夠讓機(jī)器人平衡下來。我們這里實(shí)際上用到了自動控制理論中的PID控制。PID控制是在工程中應(yīng)用最為廣泛的一種控制方法。它具有原理簡單，易于實(shí)現(xiàn)，適用面廣，控制參數(shù)相互獨(dú)立，參數(shù)的選定 比擬簡單等優(yōu)點(diǎn)。有關(guān)PID控制的詳細(xì)理論，讀者可以參考自動控制理論相關(guān)書籍。圖9.3控制方式簡圖3 3. .任務(wù)規(guī)劃前面已經(jīng)對自平衡機(jī)器人如何設(shè)計做了詳細(xì)的

7、介紹。接下去是具體實(shí)施環(huán)節(jié):搭建出實(shí)驗(yàn)平臺，這個平臺必須符合上述理論分析。熟悉所需要用到的部件， 比方CDS550解口紅外測距傳感器，可能還需要做標(biāo)定工作。在實(shí)驗(yàn)平臺上設(shè)計控制算法，對控制策略進(jìn)行驗(yàn)證。9.39.3搭建兩輪平衡機(jī)器人我們搭建的機(jī)器人如圖9.4所示。圖9.4雙輪自平衡機(jī)器人上圖中，雙輪自平衡機(jī)器人主要由兩個“輪腿、兩個連接紅外傳感器的連接臂、一個擺錘、控制器和一個連接兩輪腿的“支撐梁組成。設(shè)計控制算法是最為關(guān)鍵的一個環(huán)節(jié),的。我們可能會屢次失敗，但是相信最終一定會成功支撐機(jī)器人的機(jī)構(gòu)是由CDS55xx和輪子構(gòu)成，CDS55xx工作電機(jī)模式。輪子通過兩個結(jié)構(gòu)件連接到控制器外殼上，組

8、成兩個“輪腿。這里為什么要把控制器架高呢？之所以這樣做，有兩方面的原因：第一，根據(jù)5.3.1節(jié)中的內(nèi)容，我們可以知道，紅外測距傳感器的最正確工作范圍是在1020cm之間此時其線性度和靈敏度都比擬高，因此，我們需要把傳感器抬高到這個距離之內(nèi)；第二，為了抬高 整個構(gòu)型的重心，使機(jī)器人更容易平衡。至于為什么重心高就比擬容易平衡，我們可以想象在自己的手掌上立一根筷子與立一根竹竿相比，那個更容易些，有興趣的讀者可以參考相關(guān)的文獻(xiàn)資料或者自己做一下運(yùn)動學(xué)分析，這里不展開討論。將兩個紅外傳感器對稱的安裝在控制器的另外兩側(cè)，讓它們通過測量與地面之間的距離來感知機(jī)器人的傾斜程度，并根據(jù)傾斜程度的大小來調(diào)節(jié)兩個輪

9、子的速度，使機(jī)器人保持平衡。實(shí)際上只使用一個紅外傳感器也可以實(shí)現(xiàn)機(jī)器人的自平衡讀者可以思考一下原因，但是為了保持機(jī)器人的重力平衡以及使用下文將介紹的“差分式PID算法，我們使用兩個安裝在對稱位置的紅外測距傳感器。由第三個舵機(jī)和相應(yīng)結(jié)構(gòu)件組成的“擺錘，其主要作用是為了更進(jìn)一步地調(diào)節(jié)機(jī)器人重心的平衡，當(dāng)機(jī)器人往一邊傾倒時， 使擺錘向相反的方向擺動，從而減小甚至抵消機(jī)器人的傾倒力矩。下面，我們就雙輪自平衡機(jī)器人的搭建方法做詳細(xì)介紹。1 1. .搭建輪腿首先按圖9.5所示搭建舵機(jī)組合部件。這里需要注意的是，在安裝舵機(jī)之前需要在舵機(jī) 架內(nèi)事先放入兩個LX2連接件和螺母以方便后續(xù)安裝。圖9.5搭建舵機(jī)組合

10、件然后按圖9.6所示搭建輪子。為了增加輪子與地面的摩擦力，可以在兩個輪子上套上兩個創(chuàng)意之星套件提供的橡膠圈。圖9.6搭建輪子然后按圖9.7所示搭建“腿部。為了增加結(jié)構(gòu)剛度， 腿的支撐件由兩個I7結(jié)構(gòu)件疊加 組成。圖9.7搭建“腿部最后，按圖9.8所示搭建完整“輪腿圖9.8搭建“輪腿2 2. .搭建傳感器連接臂傳感器連接臂的搭建方法比擬簡單，如圖9.9所示。圖9.9搭建傳感器連接臂3 3. .搭建擺錘擺錘的搭建方法也比擬簡單，如圖9.10所示。圖9.10搭建擺錘4 4. .搭建支撐梁支撐梁的作用是連接兩個輪腿，增強(qiáng)結(jié)構(gòu)剛度，搭建方法如圖9.11所示，需要注意的是，兩個L2-1結(jié)構(gòu)件之間只相距2個

11、花鍵孔的距離。圖9.11搭建支撐梁5 5. .組裝完整的機(jī)器人如圖9.12所示，組裝控制器、輪腿、傳感器連接臂、擺錘和支撐梁。需注意的是由于支撐梁的長度缺乏， 需要在支撐梁兩端加上一個LD1和一個LD2連接件作為匹配。 此外，為 了保證機(jī)器人的重心不偏離幾何中心， 擺錘上使用的舵機(jī)安裝前必須首先恢復(fù)中位，安裝時需要盡量保證豎直。圖9.12搭完整機(jī)器人至此，我們的雙輪自平衡機(jī)器人就搭建完成了。由于自平衡機(jī)器人我們在搭建構(gòu)型時一定要有耐心并且注意思考安裝的順序，防止重復(fù)勞動，另外，需要加螺釘螺母的地方一定要加上，不要圖省事而誤了大事。6 6 .4.4讓機(jī)器人平衡的站起來9.4.1修改舵機(jī)的參數(shù)在默

12、認(rèn)的，f#況下，CDS55xx舵機(jī)數(shù)據(jù)返回的方式為“All return所有返回，也就是 說，當(dāng)舵機(jī)接收到任意一條指令后，都會將該指令原樣返回給控制器，以便控制器確認(rèn)舵機(jī)已經(jīng)接收到了指令。但是這樣的“問答通訊方式會占用較多控制器的通訊時間，并且使舵機(jī)的響應(yīng)速度變慢。 在本章的應(yīng)用中，控制器需要做高負(fù)荷的PID運(yùn)算，舵機(jī)的響應(yīng)速度也 是越快越好。因此我們需要修改一下舵機(jī)的數(shù)據(jù)返回方式，將默認(rèn)的“All return改成“Readreturn，這樣舵機(jī)只有在接收到讀取數(shù)據(jù)的指令后才會返回相應(yīng)數(shù)據(jù)，其它情況下不返回。連接舵機(jī)，多功能調(diào)試器和PC機(jī)，給多功能調(diào)試器接入直流電源，運(yùn)行RobotServo

13、Terminal軟件。輸入正確的端口號，保持默認(rèn)參數(shù)，點(diǎn)擊“Search掃描舵機(jī)，如圖9.13所示。掃描結(jié)束，點(diǎn)擊“Stop按鈕。電機(jī)選中列表中要設(shè)置屬性的舵機(jī)，在“Operate操 作界面下的Primary Set下Return后的下拉列表中選擇1-Read return，然后點(diǎn) 擊其后面的“Set按鈕，即可完成設(shè)置，如圖9.14所示。圖9.14設(shè)置屬性9.4.2紅外測距傳感器的標(biāo)定由于一致性的原因， 即使測量同樣的距離， 兩個紅外傳感器獲取的值也肯定不相同,圖9.13掃描舵機(jī)覦器人應(yīng)和重期潴.我們編寫控制程序之前， 需要事先獲取機(jī)器人在平衡位置時兩個傳感器的值，然后采集機(jī)器人在某一位置時的

14、傳感器的值并與平衡時的值相比擬就可以計算出此時機(jī)器人傾斜的程度。因此，需要對傳感器進(jìn)行標(biāo)定，也就是記錄下在平衡位置時傳感器的讀數(shù)。標(biāo)定的方法如下:1 .連接多功能調(diào)試器，控制器和PC機(jī)，將兩個紅外測距傳感器分別插到控制器的AD1口和AD7口，翻開控制器電源。2.通過調(diào)試器將控制器連接上PC,翻開NorthSTAR軟件，單擊菜單工具/查詢傳感器圖9.15,彈出傳感器查詢對話框圖9.16。3.將多功能調(diào)試器設(shè)置為AVRISP模式，單擊“啟動效勞按鈕，下載效勞程序。4.將多功能調(diào)試器設(shè)置為RS-232模式，在COMC本框輸入正確的端口號，將波特率選 擇57600,點(diǎn)擊“翻開翻開相應(yīng)串口，然后單擊“查

15、詢AD1按鈕，便可看到當(dāng)前各個AD通道的采樣數(shù)值。由匚編助0P圖9.15選擇菜單5.將機(jī)器人固定在地面上，盡可能使兩個紅外測距傳感器處于統(tǒng)一高度，即讓機(jī)器 人處于平衡曰文聲如SISI甥5 5交曷皿流置后Wnr!Wnr!- - I I四比機(jī)器人廂;K K白熱岸1 1口nteltel TftTft TflTfl*拜止3 3Ctrl+TCtrl+T盛麗旬弓如豆同傳感第5dlm5dlm文件QiQi : :M M目罡史函數(shù)口第肚花 9Tools-QueryAD-IO圖9.16在線查看AD采樣值的位置，屢次記錄此時兩個傳感器AD0和AD7的讀數(shù)值。然后計算 屢次記錄數(shù)據(jù)的平均值作為對應(yīng)紅外傳感器的標(biāo)定值。

16、至此，紅外測距傳感器的標(biāo)定工作完成。此處得到的標(biāo)定值在后面編程時使用。9.4.3數(shù)字式PID算法的實(shí)現(xiàn)PID調(diào)節(jié)，即“比例微分積分調(diào)節(jié)，是自動控制領(lǐng)域技術(shù)比擬成熟而且應(yīng)用最為廣泛 的一種調(diào)節(jié)方式。本節(jié)將使用數(shù)字式PID算法來實(shí)現(xiàn)自平衡機(jī)器人的控制程序。1 1 . .背景知識介紹由于本節(jié)所使用的PID算法是非常成熟的理論, 識。數(shù)字式PID算法公式如下：采樣周期 積分周期;微分周期上式的輸出只是控制量的增量Uk,因此又稱為“增量式PID算法。當(dāng)控制系統(tǒng)采用恒定的采樣周期時，第k次采樣時刻的輸出值uk可以由下式求得:在本章的應(yīng)用中，Uk對應(yīng)的就是舵機(jī)的速度，ek對應(yīng)的就是兩個紅外傳感器讀數(shù)與平衡位

17、置讀數(shù)的偏差值，這里使用 “差分 的概念，其根本思想就是利用兩個紅外傳感器讀數(shù)的 “差值 作為偏差值，也就是PID的輸入值。PID算法中最重要的三個系數(shù)Kp、Ki和Kd因此這里只介紹所需要用的結(jié)論性知.： u(k)=u(k) - u(k - 1) = Kp ek- ekT一ekTITD(ek- 2 ekek _2)式中KPKP(Lek-ekT.:2ekTITLek kKiek k，KdL2ek kek -2采樣序號，k=0,1,2Uk第k次采樣時刻的計算機(jī)輸出值;ek第k次采樣時刻輸入的偏差值;TITDKP比例系數(shù);Ki積分系數(shù)，Ki=KpT/；11Kd微分系數(shù)，Kd=KpTd/T；Uo開始進(jìn)

18、行PID控制時的原始初值;確實(shí)定。2 2. .算法的實(shí)現(xiàn)及程序編寫前面章節(jié)中，我們編寫機(jī)器人程序時，都用NorthSTAR來實(shí)現(xiàn)。NorthSTAR簡單易用，適合編寫簡單的程序。編寫復(fù)雜的程序時，NorthSTAR需要創(chuàng)立的模塊很多，程序的可讀性變差，程序編寫的難度增大。這里我們介紹一款非常好用的程序開發(fā)工具Eclipse。Eclipse是著名的跨平臺自由集成開發(fā)環(huán)境IDE,它擁有眾多的插件，這使得其擁有其 它功能相對固定的IDE軟件很難具有的靈活性。Eclipse最初主要作為Java語言的開發(fā)工具， 現(xiàn)在通過配置不同的插件可以使其作為c+，Python等語言的開發(fā)工具。如果給Eclipse配

19、置CDT插件和AVR插件，就可以構(gòu)建一個AVR集成開發(fā)環(huán)境。Eclipse的各插件都有很詳細(xì)的文檔支持，許多特性還在不斷白加強(qiáng)和擴(kuò)展。Eclipse提供了代碼關(guān)鍵字著色， 輸入代碼智能提示，代碼輸入錯誤檢查等非常強(qiáng)大的代碼編輯功能，可以極大的提高編寫代碼的效率。我們選用Eclipse作為開發(fā)工具，不但能免費(fèi)享用其當(dāng)前的強(qiáng)大功能，還可以免費(fèi)升級更新。 在編寫復(fù)雜的程序時，例如比賽程序，有這樣一款功能強(qiáng)大的開發(fā)工具，可以到達(dá)事半功倍的效果。本章，我們用Eclipse來開發(fā)自平衡機(jī)器人的程序。有關(guān)Eclipse的配置、使用方法，請參考本書配套光盤中的文檔?Eclipse For AVR程序開發(fā)?。我

20、們?nèi)绻麖?開始編寫自平衡機(jī)器人的程序，那將是一個非常漫長而復(fù)雜的過程。我們需要具備單片機(jī)編程的知識，需要了解ATmega12切片機(jī)，更需要創(chuàng)立初始化硬件、舵機(jī)控制、AD傳感器采樣和獲取、中斷效勞等多個功能模塊函數(shù) ，這樣我們就無法把注意力集 中到本章的主要目標(biāo)上讓自平衡機(jī)器人站起來。這里，我們可以通過修改“創(chuàng)意之星配套資源中提供的MultiFLEX?2-AVR控制器源代碼來實(shí)現(xiàn)?？刂破髟创a由專業(yè)工程師開發(fā)，封裝了硬件初始化、舵機(jī)控制、AD傳感器采樣和獲取、中斷效勞等所有“創(chuàng)意之星套件功能的函數(shù)，我們只需要在主函數(shù)中調(diào)用相應(yīng)的函數(shù)，即可實(shí)現(xiàn)我們想要的任何功能。接下來我們來實(shí)際開發(fā)自平衡機(jī)器人的

21、程序。數(shù)字式PID算法需要高負(fù)荷的運(yùn)算和恒定的傳感器采樣周期。為了保證傳感器的采樣周期，我們需要借助MultiFLEX?2-AVR控制器的核心一一Atmega128的定時器來實(shí)現(xiàn)。采樣結(jié)束后， 需要計算機(jī)器人當(dāng)前位置和平衡位置的偏差，然后根據(jù)此偏差產(chǎn)生抵消傾倒力矩的速度并控制舵機(jī)運(yùn)動。方便起見，我們直接將采樣、 計算、 舵機(jī)運(yùn)動控制放在定時器的中斷處理函數(shù)中。定時器是單片機(jī)提供的一種計時設(shè)備，通過對應(yīng)的控制存放器，可以設(shè)置定時長短，以及定時溢出的處理。通過這種設(shè)備， 我們可以以準(zhǔn)確的周期執(zhí)行需要的操作。例如，假定我們需要每2.5ms進(jìn)行一次AD采樣，我們就可以通過存放器將定時器的時長設(shè)置為2.

22、5ms，然后在其中斷處理函數(shù)中進(jìn)行AD采樣操作。定時器啟動后，開始計時，每當(dāng)時長等于2.5ms時，執(zhí)行一次中斷處理函數(shù)中的操作，如此反復(fù)執(zhí)行。程序的邏輯如圖9.17所示。主流程中我們只進(jìn)行硬件初始化和修改舵機(jī)模式，然后就 執(zhí)行While循環(huán)，該循環(huán)中什么都不做，只有一個1ms的延時，目的是不讓主函數(shù)退出，讓程序一直運(yùn)行。中斷處理函數(shù)中，我們對紅外測距傳感器進(jìn)行采樣，通過PID計算產(chǎn)生舵機(jī)速度和擺錘位置，然后控制舵機(jī)運(yùn)動。由于控制器一次只能對一路傳感器進(jìn)行采樣，所以中斷處理函數(shù)的中采用輪換的方式來實(shí)現(xiàn)兩路紅外測距傳感器的采樣。初次進(jìn)入中斷處理函數(shù)時，設(shè)定一個“是否對1號紅外采樣的標(biāo)志并設(shè)置為0,

23、當(dāng)該標(biāo)志為0時，對1號紅外采樣，同時認(rèn)為對2號紅外采樣完畢，讀取2號紅外的彳1第1次時，實(shí)際上沒有對2號紅外采樣，執(zhí)行完1次后，讀取的值有效，設(shè)置標(biāo)志值為1。經(jīng) 過2.5ms后，再次進(jìn)入中斷處理函數(shù)，此時標(biāo)志值為1,所以對2號紅外采樣，認(rèn)為1號紅外采樣完畢，讀取1號紅外的值，然后設(shè)置標(biāo)志為0。此時認(rèn)為1號和2號紅外數(shù)據(jù)都已經(jīng)更新，所以進(jìn)行PID調(diào)節(jié)，設(shè)置舵機(jī)速度。再經(jīng)歷2.5ms進(jìn)入中斷處理函數(shù)后，標(biāo)志為0,又開始對2好紅外采樣，讀取1號紅外的值并設(shè)置標(biāo)志位1。如此不斷反復(fù)，就可以以穩(wěn)定 的周期獲取兩路紅外測距傳感器的值并進(jìn)行PID調(diào)節(jié)。這里的1號紅外對應(yīng)AD1,2號紅外對應(yīng)AD7所謂采樣，是

24、指周期性地以某一規(guī)定間隔截取模擬信號，從而將模擬信號變換為數(shù)字信號的過程。這里我們可以簡單的理解為讀取紅外傳感器值的過程。圖9.17程序流程圖設(shè)計好程序邏輯后，我們來實(shí)際編碼。參考?Eclipse For AV就序開發(fā)?配置Eclipse ,然后運(yùn)行Eclipse,選擇file/import菜單，導(dǎo)入控制器源代碼工程FractalMaster,如圖9.18所示。圖9.18導(dǎo)入工程FractalMaster工程里設(shè)定的定時器3周期為2.5ms ,我們在定時器3的中斷函數(shù)中進(jìn) 行傳感器采樣、PID計算和舵機(jī)運(yùn)動控制。定時器3的中斷處理函數(shù)ISRSYSTEM_TIMER_INF,SR_NOBLOC向

25、于Drivers文件夾下 的Isr.c文件中，如圖9.19所示。圖9.19中斷處理函數(shù)在中斷處理函數(shù)前面添加如下全局變量:/當(dāng)前的偏差值/上一次的偏差值/上上次的偏差值/目標(biāo)增量/上一次目標(biāo)增量比例系數(shù)/積分系數(shù)微分系數(shù)/1號紅外值對應(yīng)0號AD端口/2號紅外值對應(yīng)7號AD端口/平衡時1號紅外值，該值為9.4.2節(jié)標(biāo)定得到的值/平衡時2號紅外值，該值為9.4.2節(jié)標(biāo)定得到的值。修改中斷處理函數(shù)，代碼如下:=Hi=Hi： MMiMMi ! !1 1 “: :，r r I*I*= = FrFr uTuT：，i i：j j EiMrO.MaLraL.riBLibKaEEiriEiMrO.MaLraL.

26、riBLibKaEEiri iTIHM_Elk_1iiTIHM_Elk_1i r rcimirv_ca!cimirv_c= -1 & Uk 1023)/限制值大小Uk = 1023;else if(Uk 1)/如果是逆時針傾倒speedl = Uk;speed2 = Uk|(0 x012); elseUk = -Uk;speedl = Uk|(0 x012);MFSetServoRotaSpd(1,speed1);MFServoAction();_delay_us(200);MFSetServoRotaSpd(2,speed2);MFServoAction();_delay_us(20

27、0);/1號舵機(jī)以speedl的速度轉(zhuǎn)動/執(zhí)行動作/延日200us/2號舵機(jī)以speed2的速度轉(zhuǎn)動/延日200usMFSetServoPos(3,angle3,512);MFServoAction();timer0_control.pEnableInterrupt(TIMER8_IMR_OI); 恢復(fù)定時器0中斷我們再介紹一下這里用到的幾個函數(shù):void adc_control.pStartConvert(uint8 channel)-開始一次AD采樣，參數(shù)channel為通道號0-7。uint8 adc_control.pGetRegValue (uint16* item);讀取上次AD轉(zhuǎn)

28、換的數(shù)值，16位無符號整形變量指針item是所要存放數(shù)據(jù)的變量 的地址，成功那么返回1,否那么返回0。這兩個函數(shù)是adc_control結(jié)構(gòu)體的成員。這個結(jié)構(gòu)體類似于C+蟲類的實(shí)例對象，負(fù)責(zé)管理AD初始化和采樣等與AD相關(guān)的工作。這里采用了面向?qū)ο蟮木幊趟枷耄瑢W(xué)過C+等面向?qū)ο笳Z言的讀者，很容易理解這個概念。有關(guān)面向?qū)ο蟮母嘀R，讀者可以參考專 業(yè)書籍。voidvoid MFSetServoPo$MFSetServoPo$int32 inID,int32 inPos,int32 inSpeed)- 控制ID為inID的舵機(jī)以inSpeed的速度運(yùn)動到inPos的位置，用于舵機(jī)模式voidvo

29、id MFSetServoRotaSpdMFSetServoRotaSpdint32 inID,int32 inSpeed)控制ID為inID的舵機(jī)以inSpeed的速度運(yùn)動，用于電機(jī)模式voidvoid MFServoActionMFServoAction()發(fā)送播送指令，讓所有舵機(jī)執(zhí)行接收到的運(yùn)動指令這三個函數(shù)我們在NorthSTAR生成的代碼中已經(jīng)見過了，這里不再介紹。需要注意的是,在Isr.c中只需要參加全局變量和修改ISR(SYSTEM_TIMER_INT, ISR_NOBLO喇，其他的內(nèi)容無需彳改。ISR函數(shù)為Atmega128的定時器3的中斷處理函數(shù)，定時初始設(shè)置為2.5ms,即

30、程序啟動后，每2.5ms執(zhí)行一次該函數(shù)。定時器的設(shè)置可以在Drivers文件夾下的Timer.c文件中的InitTimer3InitTimer3函數(shù)中實(shí)現(xiàn)，感興趣的讀者可以自行查看。/1號舵機(jī)正轉(zhuǎn)/2號舵機(jī)反轉(zhuǎn)/擺錘順時針擺動/如果是順時針傾倒值變正/1號舵機(jī)反轉(zhuǎn)/2號舵機(jī)正轉(zhuǎn)/擺錘逆時針擺動3號舵機(jī)以512的速度運(yùn)動到angle3的位置上面代碼中的比例系數(shù)KpK積分系數(shù)Ki、積分系數(shù)Kd的取值17、0.01、0.1 ,是在調(diào) 試過程中屢次試驗(yàn)得到的，和機(jī)器人的結(jié)構(gòu)，舵機(jī)的性能、傳感器的性能密切相關(guān)。我們在調(diào)試自己搭建的自平衡機(jī)器人時，這三個參數(shù)的取值一定要根據(jù)實(shí)際情況進(jìn)行調(diào)節(jié)，這里的值僅供參

31、考。PID參數(shù)調(diào)節(jié)的專業(yè)術(shù)語叫做PID參數(shù)的整定，所謂整定實(shí)際上就是確定調(diào)節(jié)器的比例 系數(shù)Kp、積分時間Ti和和微分時間Tdo有了這三個值，根據(jù)前面的公式，就可以計算出積 分系數(shù)Ki和微分系數(shù)Kd。這三個值一般可以通過理論計算來確定，但誤差太大。目前，應(yīng) 用最多的還是工程整定法：如經(jīng)驗(yàn)法、衰減曲線法、臨界比例帶法和反響曲線法。經(jīng)驗(yàn)法又叫現(xiàn)場湊試法， 即先確定一個Kp和Ti值，Td為0,然后查看機(jī)器人運(yùn)行效果， 經(jīng)反復(fù)修改試驗(yàn)找到比擬好的Kp和Ti值。然后參加微分作用。由于微分作用有抵抗偏差變 化的能力，所以確定一個Td值后，可把整定好的Kp和Ti值減小一點(diǎn)再進(jìn)行現(xiàn)場湊試，直 到Kp、Ti和Td

32、取得最正確值為止。顯然用經(jīng)驗(yàn)法整定的參數(shù)是準(zhǔn)確的。但花時間較多。衰減曲線法、臨界比例法和反響曲線法，都依賴于系統(tǒng)響應(yīng)曲線。而這里機(jī)器人的響應(yīng)曲線是無法繪制的，所以無法有效使用，有興趣的讀者可以查閱相關(guān)資料獲取更多的信息， 這里不做詳細(xì)介紹。修改完中斷處理函數(shù)后，我們再來修改主函數(shù)。主函數(shù)位于Apps文件夾下的Debug.c文件中，如圖9.20。圖9.20主函數(shù)修改主函數(shù)，代碼如下：#include Apps/SystemTask.h static uint8 servo_node_mapping3 = 1,2, 3;int main(void)MFInit();/硬件初始化servo_control.