2013年全國大學(xué)生電子設(shè)計大賽論文 本科組 簡易旋轉(zhuǎn)倒立擺及控制裝置(C題)

簡易旋轉(zhuǎn)倒立擺及控制裝置（C題）【本科組】1系統(tǒng)方案本系統(tǒng)主要由主控模塊、角度傳感器模塊、機(jī)械結(jié)構(gòu)模塊、電源模塊等幾部分組成，下面分別論證這幾個模塊的選擇。1.1角度傳感器模塊的論證與選擇方案一：采用絕對值式旋轉(zhuǎn)編碼器。絕對值式編碼器的每一個位置對應(yīng)一個確定的數(shù)字碼，因此它的示值只與測量的起始和終止位置有關(guān)，而與測量的中間過程無關(guān)。但是絕對值式編碼器體積和重量不太理想，安裝后可能會對電機(jī)有較大的干擾，而且絕對值編碼器相對昂貴，性價比不高。方案二：采用增量式編碼器。增量式編碼器是將位移轉(zhuǎn)換成周期性的電信號，再把這個電信號轉(zhuǎn)變成計數(shù)脈沖，用脈沖的個數(shù)表示位移的大小。增量式編碼器不能定位，但是O位的確定可以通過軟件解決，而且增量式編碼器價格低廉易得，反饋的信號可以滿足題目的要求。綜合考慮以上兩種方案，選擇方案二，采用增量式編碼器。1.2電機(jī)的論證與選擇方案一：采用步進(jìn)電機(jī)。步進(jìn)電機(jī)將電脈沖信號轉(zhuǎn)變?yōu)榻俏灰?，電機(jī)的轉(zhuǎn)速，停止的位置只取決于脈沖信號的頻率和脈沖數(shù)，因此可以通過控制脈沖頻率來控制電機(jī)轉(zhuǎn)動的速度和加速度。但是步進(jìn)電機(jī)在高速運轉(zhuǎn)下，開環(huán)會產(chǎn)生丟步的情況，開環(huán)控制情況不盡如人意，不是首選的電機(jī)。方案二：采用360度旋轉(zhuǎn)舵機(jī)。舵機(jī)為隨動系統(tǒng)，運動時可以外接較大的轉(zhuǎn)動負(fù)載，輸出扭矩大，而且抗抖動性很好。360度旋轉(zhuǎn)舵機(jī)內(nèi)置驅(qū)動電路，用單片機(jī)輸出的信號可以直接驅(qū)動，接線方便，但是舵機(jī)的轉(zhuǎn)速普遍很低，加速度不能達(dá)到理想的要求。方案三：采用帶速度閉環(huán)的直流電機(jī)。我們對于直流電機(jī)的數(shù)學(xué)模型較為熟悉，而且能夠比較精確地控制位置，速度，和加速度，只要在選擇直流電機(jī)型號的時候注意選擇額定參數(shù)較大的電機(jī)，能夠輸出足夠的轉(zhuǎn)矩帶動負(fù)載就可以達(dá)到要求。而且?guī)俣乳]環(huán)的直流電機(jī)可以綜合考慮采用方案三。1.3電機(jī)驅(qū)動模塊的論證與選擇方案一：采用購買的L298N集成芯片，該集成電路帶散熱器和光電耦合器，可以保護(hù)單片機(jī)不受電機(jī)的影響，最大程度的保護(hù)單片機(jī)。但是經(jīng)過我們連線調(diào)試之后，發(fā)現(xiàn)該種芯片一個致命的缺點，在PWM輸入達(dá)到KHZ級別的時候，輸出端波形嚴(yán)重畸變甚至直接輸出高電平，導(dǎo)致電路不能運行在所給的頻率下，經(jīng)分析可能是光耦本身截止頻率就在KHZ級別，一端輸出大電壓之后，帶寬降低，不能滿足要求。方案二：采用L298N芯片，自己制作L298N驅(qū)動模塊，因為L298N驅(qū)動電路較簡單，外圍元器件低廉易得。在將自己制作的L298N電路連線調(diào)試之后，我們發(fā)現(xiàn)完全可以在程序要求的頻率下工作，雖然死區(qū)電壓有些不近人意，低速是電機(jī)會有抖動現(xiàn)象，但是總體來說，情況比較樂觀。綜合考慮采用方案二。主控模塊采用XS128最小系統(tǒng)板，因為XS128作為飛思卡爾常用芯片，其PWM波有8個輸出通道，2理論分析與計算2.1機(jī)械結(jié)構(gòu)設(shè)計的分析2.1.1擺桿由于直流電機(jī)的扭矩輸出并不是很大，因此要求負(fù)載很輕甚至能夠忽略為空載，可以為計算和控制帶來很大的簡便，因此選擇了力學(xué)性能優(yōu)異的新材料——碳纖維，它的比重不到鋼的1/4，但強(qiáng)度是鋼的7~9倍，抗拉彈性模量為230~430Gpa亦高于鋼。而且中空碳纖維擺桿，是比較理想的擺桿模型。2.1.2底座由于自己加工制作的連接不能精確的達(dá)到完全對稱，導(dǎo)致電機(jī)不在底座的中央，因此我們選擇質(zhì)量較大的鋼材來做底座，并焊接一個港版來固定底座，防止傾倒。然后用兩個固定夾固定在桌子邊緣。經(jīng)過測試，可以承受電機(jī)高速旋轉(zhuǎn)帶來額離心力的作用。2.2電機(jī)物理模型的分析與計算2.2.1直流電機(jī)的機(jī)械特性直流電機(jī)在穩(wěn)態(tài)運行下，有下列方程式：公式1公式2 公式32.2.2直流電機(jī)的動態(tài)特性分析電機(jī)傳動系統(tǒng)的運動方程式為：公式4機(jī)械特性方程式為：公式5由公式1和公式2可得出轉(zhuǎn)速隨時間變化的規(guī)律為：公式6由上述分析可以得出結(jié)論：直流電機(jī)的物理模型為一階慣性系統(tǒng)。2.2.3直流電機(jī)加速度分析與計算由公式4求導(dǎo)可以得到：公式7上式中加速度為a，公式4經(jīng)變換可得到加速度的表達(dá)式：公式8再結(jié)合直流電機(jī)機(jī)械特性的三個方程式可以得到電機(jī)電壓與加速度的關(guān)系如下式所示：公式92.3擺桿運動狀態(tài)的分析及控制思路2.3.1擺桿起振擺桿從靜止到振蕩是一個需要擺動幅度逐漸增加的過程。而對于電機(jī)的控制，是通過檢測到反饋到單片機(jī)的擺桿擺動的角度，然后單片機(jī)輸出PWM控制一個小的加速度給電機(jī)，這個加速度擺桿向反方向擺動更大的角度?；谝陨系某绦蛟O(shè)計，結(jié)合分析了直流電機(jī)的物理模型之后，經(jīng)過了一系列簡化計算，我們認(rèn)為電機(jī)的加速度幾乎正比于控制電機(jī)的電壓，電壓大小可以通過單片機(jī)輸出的PWM波的占空比的改變而改變的。因此我們根據(jù)反饋的擺動角度，算出響應(yīng)的加速度控制電機(jī)加速轉(zhuǎn)動，這樣擺動角度越大，擺桿收到電機(jī)所給的加速度越大，形成一個正反饋環(huán)節(jié)，使擺桿逐步擺動幅度增大，實現(xiàn)起振。特意要說明的是，由于擺桿在自然下垂?fàn)顟B(tài)時檢測到的角度為0，因此按照以上推論，此時單片機(jī)輸出的加速度為0，這樣擺桿永遠(yuǎn)無法起振。為了解決這個問題，我們在擺桿處于自然下垂?fàn)顟B(tài)擺桿處于0位時，我們在程序中在此刻增加一個很小的角度，使得即使在0位，也可以通過控制算法輸出一個小的控制量，來實現(xiàn)最初的起振。2.3.2擺桿做圓周運動當(dāng)擺桿起振后由于正反饋環(huán)的存在，電機(jī)通過旋轉(zhuǎn)臂給擺桿做功，擺桿不斷獲取能量，擺桿擺桿擺動幅度越來越大，直到超過180度，能夠完成一系列圓周運動。2.3.3擺桿達(dá)到倒立平衡狀態(tài)當(dāng)擺桿能夠擺到接近于180度的位置時（正負(fù)大約15度），希望擺桿能夠趨向平衡位置（180度位置），并動態(tài)保持平衡。要實現(xiàn)上述功能，我們首先只開啟擺桿的角度反饋，不給電機(jī)控制電壓，然后用手轉(zhuǎn)動擺桿，找到第一個Z相信號，并將之定在180的平衡位置上。然后每經(jīng)過一個Z相信號，都會將角度標(biāo)準(zhǔn)重新標(biāo)定一下（標(biāo)定0度），之后我們發(fā)現(xiàn)這個標(biāo)定很有必要，因為擺桿在最低點的往復(fù)擺中，由于方向時刻改變，而測速脈沖（編碼器A相信號和B相信號異或之后的信號，相當(dāng)于二分頻）在方向不同是會有一個相移，導(dǎo)致測速脈沖有一個累積誤差。經(jīng)過實驗，隨著擺動次數(shù)增加，累積誤差可高達(dá)10幾度，導(dǎo)致零位嚴(yán)重偏移。而Z相信號的標(biāo)定可以彌補(bǔ)這個誤差。在做完標(biāo)定之后，我們將程序設(shè)計為一開始就通過PID調(diào)節(jié)給電機(jī)一個最大的加速度，使擺桿盡量能在一個擺動周期內(nèi)擺至180度平衡位置附近，然后結(jié)束使擺桿幅度加大的正反饋環(huán)節(jié)，開始進(jìn)入到所加的加速度隨著擺桿與180度平衡位置夾角的大小成正比，但加速度的方向與夾角變化方向成反比的程序中。2.4擺桿控制量的分析與計算2.4.1擺桿起振狀態(tài)分析假設(shè)某一時刻，擺桿向左擺到了α度，此時擺桿的受力狀態(tài)如圖X所示：A點為擺桿與旋轉(zhuǎn)臂連接處，設(shè)此時擺桿收到旋轉(zhuǎn)臂的力為F，方向如圖所示，此時力F的作用是使擺桿繼續(xù)向左擺起，即增大擺桿的擺動復(fù)讀。因此可以根據(jù)此受力分析給電機(jī)一致方向的角加速度。2.4.2擺桿進(jìn)入開始調(diào)整倒立平衡的角度范圍假設(shè)圖中角度為β，則想要使擺角趨向180度，要給擺桿一個圖示方向的力F使得下式成立：公式10由于考慮的連軸的摩擦力，轉(zhuǎn)動時收到的空氣阻力，外部接線對擺桿的干擾力等等，增加一個阻尼力，使得上式變?yōu)椋汗?1在算出了角速度的控制量范圍之后，需要確定單片機(jī)提供的相應(yīng)控制量。為了比較接近實際的物理情況，我們應(yīng)用2.2中分析和計算的直流電機(jī)物理模型，以及直流電機(jī)控制電壓與加速度的關(guān)系，即公式9,。再結(jié)合我們手頭可以得到的關(guān)于直流電機(jī)的各種參數(shù)，我們很難精確地算出時間常數(shù)，只能近似，而且再加上程序的控制周期遠(yuǎn)小于時間常數(shù)，綜合考慮之后，我們選擇了忽略公式9中等號右邊的后兩項變量，也就是近似認(rèn)為直流電機(jī)的控制電壓與加速度成正比關(guān)系，這樣便于單片機(jī)給出控制信號。但是經(jīng)過實際的實驗之后，我們發(fā)現(xiàn)，用PD調(diào)節(jié)來控制電壓信號，可以使擺桿處于倒立平衡狀態(tài)，但是相當(dāng)不穩(wěn)定，還經(jīng)常出現(xiàn)電機(jī)高速旋轉(zhuǎn)的情況。因此，我們在此基礎(chǔ)上加入了旋轉(zhuǎn)臂的速度反饋。之前我們在模型的建立上忽略了公式9等號右邊的后兩項，造成了比較大的誤差?，F(xiàn)在加入了旋轉(zhuǎn)臂的速度反饋之后，就可以計算出了，可以只忽略最后一項，大大減小了誤差。同時經(jīng)過實驗證明，雙環(huán)控制的效果能達(dá)到大部分的性能指標(biāo)。3電路與程序設(shè)計3.1電路的設(shè)計3.1.1系統(tǒng)總體框圖整體系統(tǒng)由機(jī)械結(jié)構(gòu)，XS128單片機(jī)最小系統(tǒng)板，電機(jī)驅(qū)動模塊，電源模塊，異或門，計數(shù)器和D觸發(fā)器，以及帶速度反饋的直流電機(jī)和增量式編碼器組成。系統(tǒng)總體框圖如圖1所示，圖1系統(tǒng)總體框圖3.1.2系統(tǒng)電路原理圖見附件23.1.3電機(jī)驅(qū)動模塊的電路設(shè)計如圖2所示圖2電機(jī)驅(qū)動模塊的電路3.2程序的設(shè)計3.2.1程序功能描述與設(shè)計思路1、程序功能描述根據(jù)題目要求，本系統(tǒng)程序的任務(wù)如下：對檢測擺桿位置的增量編碼器1的輸入信號進(jìn)行計數(shù)和方向識別，根據(jù)擺桿的擺角發(fā)出PWM波，對電機(jī)進(jìn)行控制，同時接受外部計數(shù)器對旋轉(zhuǎn)臂速度檢測反饋回來的信息，綜合兩個反饋，根據(jù)基于物理模型得到的電機(jī)控制電壓計算公式，控制電機(jī)使給擺桿一個加速度，不同的控制算法能夠?qū)崿F(xiàn)相應(yīng)的功能，如起振，圓周運動，倒立平衡等。電機(jī)控制程序內(nèi)容：見附件33.2.2程序流程圖1、主程序與子程序框圖圖3主程序與子程序框圖2、“起擺做圓周運動”子程序流程圖圖4“起擺做圓周運動”子程序流程圖3、“起擺并盡快達(dá)到穩(wěn)態(tài)”子程序流程圖圖5“起擺并達(dá)到倒立平衡狀態(tài)”子程序流程圖4測試方案與測試結(jié)果4.1測試方案硬件軟件聯(lián)調(diào)：調(diào)試時，下載線連接單片機(jī)，以便程序執(zhí)行后實時觀測控制算法中多個變量，如擺桿角度，方向，電機(jī)速度以及加速度等等。通過撥碼開關(guān)來決定運行不同的程序，以實現(xiàn)不同的功能。相互功能之間沒有干擾。實現(xiàn)另一個功能需要單片機(jī)復(fù)位后重新運行。記錄下每次實現(xiàn)不同功能的性能指標(biāo)。4.2測試條件與儀器測試條件：支架水平放置于桌面上，保證擺桿處于自然下垂?fàn)顟B(tài)，除電機(jī)外無其他外力對擺桿造成干擾（空氣阻力，摩擦力等不算在內(nèi)）。測試儀器：直流穩(wěn)壓電源，數(shù)字示波器，數(shù)字萬用表，秒表，量角器.4.3測試結(jié)果及分析4.3.1測試結(jié)果(數(shù)據(jù))測試功能一：擺桿起振測試次數(shù)123456擺動幅度（度）>60>60>60>60>60>60擺動時間（s）212354測試功能二：擺桿完成圓周運動說明：以完整的完成一個圓周運動的時間為計時標(biāo)準(zhǔn)。測試次數(shù)12345678完成時間（s）<2<2<2<2<1<2<1>1測試功能三：擺桿盡快達(dá)到倒立平衡狀態(tài)并保持5s以上，且旋轉(zhuǎn)臂旋轉(zhuǎn)的轉(zhuǎn)動角度不大于90度。測試次數(shù)12345678保持時間（s）>5>5<5>5>5<5>5>5旋轉(zhuǎn)臂最大轉(zhuǎn)動角（度）<90<90>90<90>90>90<90>90測試功能四：從擺桿處于自然下垂?fàn)顟B(tài)開始，控制旋轉(zhuǎn)臂做往復(fù)旋轉(zhuǎn)運動，盡快使擺桿擺起倒立，保持倒立時間不小于10s測試次數(shù)12345678擺起時間（s）23--12.52．514保持時間（s）>10>100>10>10>10>10>10測試功能五：在擺桿保持倒立狀態(tài)下，施加干擾后擺桿能繼續(xù)保持倒立或2s內(nèi)恢復(fù)倒立狀態(tài)。測試次數(shù)12345678能否保持能能能能能能能否恢復(fù)時間（s）23433454測試功能六：在擺桿保持倒立狀態(tài)的前提下，旋轉(zhuǎn)臂做圓周運動，并盡快使單方向轉(zhuǎn)過角度達(dá)到或超過360度。測試次數(shù)12345678旋轉(zhuǎn)臂轉(zhuǎn)過360度時間（s）>5>5>5>5>5>5>5>54.3.2測試分析與結(jié)論根據(jù)上述測試數(shù)據(jù)，該系統(tǒng)基本上能夠達(dá)到基本部分和發(fā)揮部分(1)(3)的性能指標(biāo)，由此可以得出以下結(jié)論：1、電機(jī)的性能不是很理想，聯(lián)軸器有些松動，長時間運行兩者有些脫節(jié)，起振達(dá)到平衡很困難，2、雙環(huán)控制需要時間上的配合，加入的時間和參數(shù)很重要。3、好的硬件電路時成功的奠基石。綜上所述，本設(shè)計基本達(dá)到設(shè)計要求。5總結(jié)本次比賽中我們組遇到的最大的困難是硬件問題，包括合適的電動機(jī)的選型，先后換了三種型號的電動機(jī)，電動機(jī)與底座和旋轉(zhuǎn)臂的連接，以及能夠經(jīng)受住長時間離心力作用的聯(lián)軸器和鋼珠等支承結(jié)構(gòu)。四天三夜中很多時間在組建和檢修硬件部分，導(dǎo)致浪費了很多調(diào)試程序的時間，東奔西跑地尋找合適的材料也浪費了很多體力。在斷斷續(xù)續(xù)的將硬件做好之后，在調(diào)試軟件時我們一直采用的程序設(shè)計思路在我們攻克倒立平衡的穩(wěn)定性時，遇到了很大的挑戰(zhàn)，我們在這個程序思路上下了很多功夫，但是調(diào)整結(jié)果很不滿意。在第四天的下午，離比賽結(jié)束還有幾個小時時，我們放棄了一直堅持的程序主線——將加速度環(huán)作為內(nèi)環(huán)，速度環(huán)作為外環(huán)進(jìn)行控制，轉(zhuǎn)而采用了將速度環(huán)作為內(nèi)環(huán)，而加速度環(huán)作為外環(huán)，消除了速度的采樣滯后，才取得了比較可喜的進(jìn)步，但是由于時間實在太有限，我們沒有機(jī)會將所有部分都完美的移植在新的程序思路上。遺憾是有的，但是收獲也是巨大的，四天三夜，體驗到了很多東西，團(tuán)隊的力量，溝通的重要性，最重要的是遇到困難能夠不被得失的情緒所干擾，而是一心一意去解決問題，這是我們以后對待任何困難的一種成功的心態(tài)。在這比賽的最后，要感謝學(xué)校和學(xué)院老師的給力的后勤工作，使我們能夠在不被煩擾的情況下，用最充足的時間完成比賽。感謝全國大學(xué)生電子設(shè)計大賽的組委會，提供這樣優(yōu)秀的平臺和機(jī)會給我們充足的鍛煉。附件1：系統(tǒng)電路原理圖附件2：電機(jī)控制源程序子函數(shù)： #include"carsub.h" #include<hidef.h> #include"derivative.h" voidGetMotorPulse(void) { unsignedintnLeftPulse,nRightPulse;unsignedcharleftdir,rightdir;//unsignedcharii; leftdir=PORTB&0x01;//讀取轉(zhuǎn)向 rightdir=PORTB&0x04; nLeftPulse=PACNT;//left為內(nèi)部計數(shù)器，right為外部計數(shù)器nRightPulse=PORTA;//讀取脈沖數(shù)PACNT=0;PTJ=PTJ|0XFF;Delay(15);PTJ=PTJ&0X00;//計數(shù)器清零 if(leftdir==0){ LeftMotorPulse-=(int)nLeftPulse; LeftMotorPulseSigma-=(int)nLeftPulse; } else{ LeftMotorPulse+=(int)nLeftPulse; LeftMotorPulseSigma+=(int)nLeftPulse; } if(rightdir==0) RightMotorSpeed=-(int)nRightPulse; else RightMotorSpeed=(int)nRightPulse; //PACNT=0; //if(leftdir==0) g_lPosition=LeftMotorPulse; } voidSetMotorVoltage(intnLeftVal) { if(nLeftVal>MOTOR_OUT_MAX) nLeftVal=MOTOR_OUT_MAX; if(nLeftVal<MOTOR_OUT_MIN) nLeftVal=MOTOR_OUT_MIN; if(nLeftVal>0) { PWMDTY45=0; PWMDTY01=nLeftVal+120; } else { PWMDTY45=-nLeftVal+120; PWMDTY01=0; } } voidMotorOutput(void) { //計算最終給定的PWM占空比 //intLeftMotorOutput; //floatK,P; //if(PORTB_PB3==1) //K=350; //elseK=700; LeftMotorOutput=(int)K*PositionControlOut+SpeedControlOut; if(PORTB_PB4==0){ if((LeftMotorSpeed<=0)&&(LeftMotorSpeedOld>0)) { g_ucRight=1; g_ucLeft=0; } if((g_ucRight==1)&&((GravityAngle>0)&&(GravityAngle<60))) { LeftMotorOutput=0; //SPEED_CONTROL_P=0; //SPEED_CONTROL_D=0; } if((LeftMotorSpeed>=0)&&(LeftMotorSpeedOld<0)) { g_ucLeft=1; g_ucRight=0; } if((g_ucLeft==1)&&((GravityAngle<0)&&(GravityAngle>-60))){ LeftMotorOutput=0; } } if(PORTB_PB3==0){ if((LeftMotorSpeed<=0)&&(LeftMotorSpeedOld>0)) { g_ucRight=1; g_ucLeft=0; } if((g_ucRight==1)&&((GravityAngle>=0)&&(GravityAngle<=180))) LeftMotorOutput=-LeftMotorOutput; if((LeftMotorSpeed>=0)&&(LeftMotorSpeedOld<0)) { g_ucLeft=1; g_ucRight=0; } if((g_ucLeft==1)&&((GravityAngle<=0)&&(GravityAngle>=-180))) LeftMotorOutput=-LeftMotorOutput; } SetMotorVoltage(LeftMotorOutput); } voidPositionControl(void) { floatfP,fD; POSITION_CONTROL_P=12; POSITION_CONTROL_D=0.002; if(GravityAngle<0){ //if(GravityAngle>-45)fP=-15; //if(GravityAngle<-180)fP=-POSITION_CONTROL_P*tancode[-GravityAngle-180]; //else fP=-POSITION_CONTROL_P*tancode[-GravityAngle]; } //elseif(GravityAngle>180){ //if(GravityAngle>315) //fP=-15; //else //fP=POSITION_CONTROL_P*tancode[GravityAngle-180]; //} else{ //if(GravityAngle<45) //fP=15; //else fP=POSITION_CONTROL_P*tancode[GravityAngle]; } fD=POSITION_CONTROL_D*AngleSpeed; //fI1+=POSITION_CONTROL_I*PositionErr; PositionControlOld=PositionControlNew; PositionControlNew=fP+fD;//ＰＩＤ得到的是角度控制量，加減速由車身的前傾角度決定 }voidSpeedControl(void){floatfP,fI,fD; //RightMotorSpeedGoal=0;//RightMotorSpeed=RightMotorPulseSigma;//RightMotorPulseSigma=0; RightMotorSpeedErr0=RightMotorSpeedErr1; RightMotorSpeedErr1=RightMotorSpeedErr2; RightMotorSpeedErr2=RightMotorSpeedGoal-RightMotorSpeed;//計算速度誤差 if((PORTB_PB4==0)&&(g_ucBegin==0)) { SPEED_CONTROL_P=0;SPEED_CONTROL_I=0; } else { SPEED_CONTROL_P=120; SPEED_CONTROL_I=1; }//if(g_ucBegin==0){//SPEED_CONTROL_P=0;//SPEED_CONTROL_I=0;//SPEED_CONTROL_D=0;//} fP=SPEED_CONTROL_P*(RightMotorSpeedErr2-RightMotorSpeedErr1); fI=SPEED_CONTROL_I*RightMotorSpeedErr2;fD=SPEED_CONTROL_D*(RightMotorSpeedErr2-2*RightMotorSpeedErr1+RightMotorSpeedErr0);//SpeedControlOld=SpeedControlNew;SpeedControlOut+=fP+fI+fD;}voidAngleCalculate(void){LeftMotorSpeedOld=LeftMotorSpeed;LeftMotorSpeed=LeftMotorPulseSigma;LeftMotorPulseSigma=0;GravityAngle=((int)(GRAVITY_ANGLE_RATIO*g_lPosition))%360;if(GravityAngle>180)GravityAngle=GravityAngle-360;AngleSpeed=ANGLE_SPEED_RATIO*LeftMotorSpeed;}voidDelay(intnum){intdelay_c;for(delay_c=0;delay_c<num;delay_c++);}voidPositionSetOut(void){floatfvalue;fvalue=PositionControlNew-PositionControlOld;PositionControlOut=fvalue*(PositionControlCount+1)/POSITION_CONTROL_COUNT+PositionControlOld;}主函數(shù)： #include<hidef.h> #include"derivative.h" #include"init.h" #include"carsub.h" constfloattancode[181]={0.010,0.0175,0.0349,0.0524,0.0699,0.0875,0.1051,0.1228,0.1405,0.1584,0.1763,0.1944,0.2126,0.2309,0.2493,0.2679,0.2867,0.3057,0.3249,0.3443,0.3640,0.3839,0.4040,0.4245,0.4452,0.4663,0.4877,0.5095,0.5317,0.5543,0.5774,0.6009,0.6249,0.6494,0.6745,0.7002,0.7265,0.7536,0.7813,0.8098,0.8391,0.8693,0.9004,0.9325,0.9657,1.0000,1.0355,1.0724,1.1106,1.1504,1.1918,1.2349,1.2799,1.3270,1.3764,1.4281,1.4826,1.5399,1.6003,1.6643,1.7321,1.8040,1.8807,1.9626,2.0503,2.1445,2.2460,2.3559,2.4751,2.6051,2.7475,2.9042,3.0777,3.2709,3.4874,3.7321,4.0108,4.3315,4.7046,5.1446,5.6713,6.3138,7.1154,8.1443,9.5144,10.0000,10.0000,10.0000,10.0000,10.0000,10.0000,-10.0000,-10.0000,-10.0000,-10.0000,-10.0000,-9.5144,-8.1443,-7.1154,-6.3138,-5.6713,-5.1446,-4.7046,-4.3315,-4.0108,-3.7321,-3.4874,-3.2709,-3.0777,-2.9042,-2.7475,-2.6051,-2.4751,-2.3559,-2.2460,-2.1445,-2.0503,-1.9626,-1.8807,-1.8040,-1.7321,-1.6643,-1.6003,-1.5399,-1.4826,-1.4281,-1.3764,-1.3270,-1.2799,-1.2349,-1.1918,-1.1504,-1.1106,-1.0724,-1.0355,-1.0000,-0.9657,-0.9325,-0.9004,-0.8693,-0.8391,-0.8098,-0.7813,-0.7536,-0.7265,-0.7002,-0.6745,-0.6494,-0.6249,-0.6009,-0.5774,-0.5543,-0.5317,-0.5095,-0.4877,-0.4663,-0.4452,-0.4245,-0.4040,-0.3839,-0.3640,-0.3443,-0.3249,-0.3057,-0.2867,-0.2679,-0.2493,-0.2309,-0.2126,-0.1944,-0.1763,-0.1584,-0.1405,-0.1228,-0.1051,-0.0875,-0.0699,-0.0524,-0.0349,-0.0175,0.0000};//constfloat floatPOSITION_CONTROL_P,POSITION_CONTROL_D,POSITION_CONTROL_I; floatSPEED_CONTROL_P,SPEED_CONTROL_I,SPEED_CONTROL_D; floatGRAVITY_ANGLE_RATIO=0.5,ANGLE_SPEED_RATIO=100; floatK=650;unsignedcharg_ucBegin,g_ucRight,g_ucLeft;unsignedcharPositionControlCount; intLeftMotorPulse; //intg_iZOffset;intg_lPosition=0,g_lPositionBase=0; //longg_lPositionGoal; //floatPositionControlNew,PositionControlOld; floatPositionControlOut,PositionControlNew,PositionControlOld,SpeedControlOut; //intPositionErr=5500,PositionErrOld; floatLeftMotorSpeed,LeftMotorSpeedOld; intRightMotorPulseSigma,LeftMotorPulseSigma,RightMotorSpeed,RightMotorSpeedGoal,RightMotorSpeedGoal1; floatRightMotorSpeedErr0,RightMotorSpeedErr1,RightMotorSpeedErr2;intLeftMotorOutput;//floatfI1=0;intGravityAngle;floatAngleSpeed;/**************************主程序***************************