大學(xué)生電子設(shè)計大賽論文簡易旋轉(zhuǎn)倒立擺與控制裝置(C題)

1、PAGE24 / NUMPAGES272013年全國大學(xué)生電子設(shè)計競賽簡易旋轉(zhuǎn)倒立擺與控制裝置（C題）本科組2013年9月6日摘 要 本系統(tǒng)采用XS128單片機最小系統(tǒng)作為控制中心、與電機驅(qū)動模塊，減速直流電機、旋轉(zhuǎn)臂、擺桿、反饋裝置一起構(gòu)成擺桿角度和電機角速度的雙閉環(huán)調(diào)速系統(tǒng)。由于直流電機屬于一階慣性，經(jīng)分析在控制周期遠小于電機時間常數(shù)的情況下建立電壓與加速度的近似比例關(guān)系模型。選擇減速直流電機帶動旋轉(zhuǎn)臂旋轉(zhuǎn)，采用增量式旋轉(zhuǎn)編碼器測量擺桿的角度，單片機輸出占空比可變的PWM波控制電機角加速度，從而控制電機的加速度和給擺桿固定軸心的扭矩，實現(xiàn)控制擺桿的擺動幅度并達到和保持平衡狀態(tài)。控制方式采用

2、PID控制，比例環(huán)節(jié)進行快速響應(yīng)，積分環(huán)節(jié)實現(xiàn)無靜差，微分環(huán)節(jié)減小超調(diào)，加快動態(tài)響應(yīng)。從而使該系統(tǒng)具有良好的性能，能很好的實現(xiàn)起振、圓周運動、倒立、360度倒立旋轉(zhuǎn)，同時具有很好的抗擾動性能。關(guān)鍵詞：XS128 直流電機 加速度 倒立平衡 目 錄 TOC o 1-3 h z u HYPERLINK l _Toc366347492 1系統(tǒng)方案 PAGEREF _Toc366347492 h 1 HYPERLINK l _Toc366347493 1.1 角度傳感器模塊的論證與選擇 PAGEREF _Toc366347493 h 1 HYPERLINK l _Toc366347494 1.2電機的

3、論證與選擇 PAGEREF _Toc366347494 h 1 HYPERLINK l _Toc366347495 1.3 電機驅(qū)動模塊的論證與選擇 PAGEREF _Toc366347495 h 1 HYPERLINK l _Toc366347496 2理論分析與計算 PAGEREF _Toc366347496 h 2 HYPERLINK l _Toc366347497 2.1 機械結(jié)構(gòu)設(shè)計的分析 PAGEREF _Toc366347497 h 2 HYPERLINK l _Toc366347498 2.1.1 擺桿 PAGEREF _Toc366347498 h 2 HYPERLINK l

4、 _Toc366347499 2.1.2 底座 PAGEREF _Toc366347499 h 2 HYPERLINK l _Toc366347500 2.2電機物理模型的分析與計算 PAGEREF _Toc366347500 h 2 HYPERLINK l _Toc366347501 2.2.1 直流電機的機械特性 PAGEREF _Toc366347501 h 2 HYPERLINK l _Toc366347502 2.2.2 直流電機的動態(tài)特性分析 PAGEREF _Toc366347502 h 2 HYPERLINK l _Toc366347503 2.2.3 直流電機加速度分析與計算

5、 PAGEREF _Toc366347503 h 3 HYPERLINK l _Toc366347504 2.3擺桿運動狀態(tài)的分析與控制思路 PAGEREF _Toc366347504 h 4 HYPERLINK l _Toc366347505 2.3.1擺桿起振 PAGEREF _Toc366347505 h 4 HYPERLINK l _Toc366347506 2.3.2擺桿做圓周運動 PAGEREF _Toc366347506 h 4 HYPERLINK l _Toc366347507 2.3.3擺桿達到倒立平衡狀態(tài) PAGEREF _Toc366347507 h 4 HYPERLIN

6、K l _Toc366347508 2.4 擺桿控制量的分析與計算 PAGEREF _Toc366347508 h 4 HYPERLINK l _Toc366347509 2.4.1 擺桿起振狀態(tài)分析 PAGEREF _Toc366347509 h 4 HYPERLINK l _Toc366347510 2.4.2 擺桿進入開始調(diào)整倒立平衡的角度圍 PAGEREF _Toc366347510 h 5 HYPERLINK l _Toc366347511 3電路與程序設(shè)計 PAGEREF _Toc366347511 h 6 HYPERLINK l _Toc366347512 3.1電路的設(shè)計 PA

7、GEREF _Toc366347512 h 6 HYPERLINK l _Toc366347513 3.1.1系統(tǒng)總體框圖 PAGEREF _Toc366347513 h 6 HYPERLINK l _Toc366347514 3.1.2 系統(tǒng)電路原理圖 PAGEREF _Toc366347514 h 6 HYPERLINK l _Toc366347515 3.1.3 電機驅(qū)動模塊的電路設(shè)計 PAGEREF _Toc366347515 h 6 HYPERLINK l _Toc366347516 3.2程序的設(shè)計 PAGEREF _Toc366347516 h 7 HYPERLINK l _To

8、c366347517 3.2.1程序功能描述與設(shè)計思路 PAGEREF _Toc366347517 h 7 HYPERLINK l _Toc366347518 3.2.2程序流程圖 PAGEREF _Toc366347518 h 7 HYPERLINK l _Toc366347519 4測試方案與測試結(jié)果 PAGEREF _Toc366347519 h 11 HYPERLINK l _Toc366347520 4.1測試方案 PAGEREF _Toc366347520 h 11 HYPERLINK l _Toc366347521 4.2 測試條件與儀器 PAGEREF _Toc36634752

9、1 h 11 HYPERLINK l _Toc366347522 4.3 測試結(jié)果與分析 PAGEREF _Toc366347522 h 11 HYPERLINK l _Toc366347523 4.3.1測試結(jié)果(數(shù)據(jù)) PAGEREF _Toc366347523 h 11 HYPERLINK l _Toc366347524 4.3.2測試分析與結(jié)論 PAGEREF _Toc366347524 h 12 HYPERLINK l _Toc366347525 5總結(jié) PAGEREF _Toc366347525 h 12 HYPERLINK l _Toc366347526 附件1：系統(tǒng)電路原理圖

10、PAGEREF _Toc366347526 h 14 HYPERLINK l _Toc366347527 附件2：電機控制源程序 PAGEREF _Toc366347527 h 15簡易旋轉(zhuǎn)倒立擺與控制裝置（C題）本科組1系統(tǒng)方案本系統(tǒng)主要由主控模塊、角度傳感器模塊、機械結(jié)構(gòu)模塊、電源模塊等幾部分組成，下面分別論證這幾個模塊的選擇。1.1 角度傳感器模塊的論證與選擇方案一：采用絕對值式旋轉(zhuǎn)編碼器。絕對值式編碼器的每一個位置對應(yīng)一個確定的數(shù)字碼，因此它的示值只與測量的起始和終止位置有關(guān)，而與測量的中間過程無關(guān)。但是絕對值式編碼器體積和重量不太理想，安裝后可能會對電機有較大的干擾，而且絕對值編碼器

11、相對昂貴，性價比不高。方案二：采用增量式編碼器。增量式編碼器是將位移轉(zhuǎn)換成周期性的電信號，再把這個電信號轉(zhuǎn)變成計數(shù)脈沖，用脈沖的個數(shù)表示位移的大小。增量式編碼器不能定位，但是O位的確定可以通過軟件解決，而且增量式編碼器價格低廉易得，反饋的信號可以滿足題目的要求。綜合考慮以上兩種方案，選擇方案二，采用增量式編碼器。1.2電機的論證與選擇方案一：采用步進電機。步進電機將電脈沖信號轉(zhuǎn)變?yōu)榻俏灰?，電機的轉(zhuǎn)速，停止的位置只取決于脈沖信號的頻率和脈沖數(shù)，因此可以通過控制脈沖頻率來控制電機轉(zhuǎn)動的速度和加速度。但是步進電機在高速運轉(zhuǎn)下，開環(huán)會產(chǎn)生丟步的情況，開環(huán)控制情況不盡如人意，不是首選的電機。方案二：采用

12、360度旋轉(zhuǎn)舵機。舵機為隨動系統(tǒng)，運動時可以外接較大的轉(zhuǎn)動負(fù)載，輸出扭矩大，而且抗抖動性很好。360度旋轉(zhuǎn)舵機置驅(qū)動電路，用單片機輸出的信號可以直接驅(qū)動，接線方便，但是舵機的轉(zhuǎn)速普遍很低，加速度不能達到理想的要求。方案三：采用帶速度閉環(huán)的直流電機。我們對于直流電機的數(shù)學(xué)模型較為熟悉，而且能夠比較精確地控制位置，速度，和加速度，只要在選擇直流電機型號的時候注意選擇額定參數(shù)較大的電機，能夠輸出足夠的轉(zhuǎn)矩帶動負(fù)載就可以達到要求。而且?guī)俣乳]環(huán)的直流電機可以綜合考慮采用方案三。1.3 電機驅(qū)動模塊的論證與選擇方案一：采用購買的L298N集成芯片，該集成電路帶散熱器和光電耦合器，可以保護單片機不受電機的

13、影響，最大程度的保護單片機。但是經(jīng)過我們連線調(diào)試之后，發(fā)現(xiàn)該種芯片一個致命的缺點，在PWM輸入達到KHZ級別的時候，輸出端波形嚴(yán)重畸變甚至直接輸出高電平，導(dǎo)致電路不能運行在所給的頻率下，經(jīng)分析可能是光耦本身截止頻率就在KHZ級別，一端輸出大電壓之后，帶寬降低，不能滿足要求。方案二：采用L298N芯片，自己制作L298N驅(qū)動模塊，因為L298N驅(qū)動電路較簡單，外圍元器件低廉易得。在將自己制作的L298N電路連線調(diào)試之后，我們發(fā)現(xiàn)完全可以在程序要求的頻率下工作，雖然死區(qū)電壓有些不近人意，低速是電機會有抖動現(xiàn)象，但是總體來說，情況比較樂觀。綜合考慮采用方案二。主控模塊采用XS128最小系統(tǒng)板，因為X

14、S128作為飛思卡爾常用芯片，其PWM波有8個輸出通道，2理論分析與計算2.1 機械結(jié)構(gòu)設(shè)計的分析2.1.1 擺桿由于直流電機的扭矩輸出并不是很大，因此要求負(fù)載很輕甚至能夠忽略為空載，可以為計算和控制帶來很大的簡便，因此選擇了力學(xué)性能優(yōu)異的新材料碳纖維，它的比重不到鋼的1/4，但強度是鋼的79倍，抗拉 HYPERLINK :/baike.baidu /view/30660.htm t _blank 彈性模量為230430Gpa亦高于鋼。而且中空碳纖維擺桿，是比較理想的擺桿模型。2.1.2 底座由于自己加工制作的連接不能精確的達到完全對稱，導(dǎo)致電機不在底座的中央，因此我們選擇質(zhì)量較大的鋼材來做底

15、座，并焊接一個港版來固定底座，防止傾倒。然后用兩個固定夾固定在桌子邊緣。經(jīng)過測試，可以承受電機高速旋轉(zhuǎn)帶來額離心力的作用。2.2電機物理模型的分析與計算2.2.1 直流電機的機械特性 直流電機在穩(wěn)態(tài)運行下，有下列方程式： 公式1 公式2 公式32.2.2 直流電機的動態(tài)特性分析 電機傳動系統(tǒng)的運動方程式為： 公式4 機械特性方程式為： 公式5 由公式1和公式2可得出轉(zhuǎn)速隨時間變化的規(guī)律為：公式6 由上述分析可以得出結(jié)論：直流電機的物理模型為一階慣性系統(tǒng)。2.2.3 直流電機加速度分析與計算 由公式4求導(dǎo)可以得到： 公式7 上式中加速度為a，公式4經(jīng)變換可得到加速度的表達式： 公式8 再結(jié)合直流

16、電機機械特性的三個方程式可以得到電機電壓與加速度的關(guān)系如下式所示： 公式9 2.3擺桿運動狀態(tài)的分析與控制思路2.3.1擺桿起振擺桿從靜止到振蕩是一個需要擺動幅度逐漸增加的過程。而對于電機的控制，是通過檢測到反饋到單片機的擺桿擺動的角度，然后單片機輸出PWM控制一個小的加速度給電機，這個加速度擺桿向反方向擺動更大的角度?；谝陨系某绦蛟O(shè)計，結(jié)合分析了直流電機的物理模型之后，經(jīng)過了一系列簡化計算，我們認(rèn)為電機的加速度幾乎正比于控制電機的電壓，電壓大小可以通過單片機輸出的PWM波的占空比的改變而改變的。因此我們根據(jù)反饋的擺動角度，算出響應(yīng)的加速度控制電機加速轉(zhuǎn)動，這樣擺動角度越大，擺桿收到電機所給

17、的加速度越大，形成一個正反饋環(huán)節(jié)，使擺桿逐步擺動幅度增大，實現(xiàn)起振。特意要說明的是，由于擺桿在自然下垂?fàn)顟B(tài)時檢測到的角度為0，因此按照以上推論，此時單片機輸出的加速度為0，這樣擺桿永遠無法起振。為了解決這個問題，我們在擺桿處于自然下垂?fàn)顟B(tài)擺桿處于0位時，我們在程序中在此刻增加一個很小的角度，使得即使在0位，也可以通過控制算法輸出一個小的控制量，來實現(xiàn)最初的起振。2.3.2擺桿做圓周運動當(dāng)擺桿起振后由于正反饋環(huán)的存在，電機通過旋轉(zhuǎn)臂給擺桿做功，擺桿不斷獲取能量，擺桿擺桿擺動幅度越來越大，直到超過180度，能夠完成一系列圓周運動。2.3.3擺桿達到倒立平衡狀態(tài)當(dāng)擺桿能夠擺到接近于180度的位置時（

18、正負(fù)大約15度），希望擺桿能夠趨向平衡位置（180度位置），并動態(tài)保持平衡。要實現(xiàn)上述功能，我們首先只開啟擺桿的角度反饋，不給電機控制電壓，然后用手轉(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ù)增加，累積誤差可高達10幾度，導(dǎo)致零位嚴(yán)重偏移。而Z相信號的標(biāo)定可以彌補這個誤差。在做完標(biāo)定之后，我們將程序設(shè)

19、計為一開始就通過PID調(diào)節(jié)給電機一個最大的加速度，使擺桿盡量能在一個擺動周期擺至180度平衡位置附近，然后結(jié)束使擺桿幅度加大的正反饋環(huán)節(jié)，開始進入到所加的加速度隨著擺桿與180度平衡位置夾角的大小成正比，但加速度的方向與夾角變化方向成反比的程序中。2.4擺桿控制量的分析與計算2.4.1 擺桿起振狀態(tài)分析 假設(shè)某一時刻，擺桿向左擺到了度，此時擺桿的受力狀態(tài)如圖X所示：A點為擺桿與旋轉(zhuǎn)臂連接處，設(shè)此時擺桿收到旋轉(zhuǎn)臂的力為F，方向如圖所示，此時力F的作用是使擺桿繼續(xù)向左擺起，即增大擺桿的擺動復(fù)讀。因此可以根據(jù)此受力分析給電機一致方向的角加速度。2.4.2 擺桿進入開始調(diào)整倒立平衡的角度圍 假設(shè)圖中角

20、度為，則想要使擺角趨向180度，要給擺桿一個圖示方向的力F使得下式成立： 公式10 由于考慮的連軸的摩擦力，轉(zhuǎn)動時收到的空氣阻力，外部接線對擺桿的干擾力等等，增加一個阻尼力，使得上式變?yōu)椋?公式11 在算出了角速度的控制量圍之后，需要確定單片機提供的相應(yīng)控制量。為了比較接近實際的物理情況，我們應(yīng)用2.2中分析和計算的直流電機物理模型，以與直流電機控制電壓與加速度的關(guān)系，即公式9,。再結(jié)合我們手頭可以得到的關(guān)于直流電機的各種參數(shù)，我們很難精確地算出時間常數(shù)，只能近似，而且再加上程序的控制周期遠小于時間常數(shù)，綜合考慮之后，我們選擇了忽略公式9中等號右邊的后兩項變量，也就是近似認(rèn)為直流電機的控制電壓

21、與加速度成正比關(guān)系，這樣便于單片機給出控制信號。但是經(jīng)過實際的實驗之后，我們發(fā)現(xiàn)，用PD調(diào)節(jié)來控制電壓信號，可以使擺桿處于倒立平衡狀態(tài)，但是相當(dāng)不穩(wěn)定，還經(jīng)常出現(xiàn)電機高速旋轉(zhuǎn)的情況。 因此，我們在此基礎(chǔ)上加入了旋轉(zhuǎn)臂的速度反饋。之前我們在模型的建立上忽略了公式9等號右邊的后兩項，造成了比較大的誤差?，F(xiàn)在加入了旋轉(zhuǎn)臂的速度反饋之后，就可以計算出了，可以只忽略最后一項，大大減小了誤差。同時經(jīng)過實驗證明，雙環(huán)控制的效果能達到大部分的性能指標(biāo)。3電路與程序設(shè)計3.1電路的設(shè)計3.1.1系統(tǒng)總體框圖 整體系統(tǒng)由機械結(jié)構(gòu)，XS128單片機最小系統(tǒng)板，電機驅(qū)動模塊，電源模塊，異或門，計數(shù)器和D觸發(fā)器，以與帶

22、速度反饋的直流電機和增量式編碼器組成。系統(tǒng)總體框圖如圖1所示，圖1 系統(tǒng)總體框圖3.1.2 系統(tǒng)電路原理圖 見附件23.1.3 電機驅(qū)動模塊的電路設(shè)計 如圖2所示 圖2 電機驅(qū)動模塊的電路 3.2程序的設(shè)計3.2.1程序功能描述與設(shè)計思路1、程序功能描述根據(jù)題目要求，本系統(tǒng)程序的任務(wù)如下：對檢測擺桿位置的增量編碼器1的輸入信號進行計數(shù)和方向識別，根據(jù)擺桿的擺角發(fā)出PWM波，對電機進行控制，同時接受外部計數(shù)器對旋轉(zhuǎn)臂速度檢測反饋回來的信息，綜合兩個反饋，根據(jù)基于物理模型得到的電機控制電壓計算公式，控制電機使給擺桿一個加速度，不同的控制算法能夠?qū)崿F(xiàn)相應(yīng)的功能，如起振，圓周運動，倒立平衡等。電機控制

23、程序容：見附件33.2.2程序流程圖1、主程序與子程序框圖 圖3 主程序與子程序框圖2、“起擺做圓周運動”子程序流程圖 圖4 “起擺做圓周運動”子程序流程圖3、“起擺并盡快達到穩(wěn)態(tài)”子程序流程圖 圖5 “起擺并達到倒立平衡狀態(tài)”子程序流程圖4測試方案與測試結(jié)果4.1測試方案硬件軟件聯(lián)調(diào)：調(diào)試時，下載線連接單片機，以便程序執(zhí)行后實時觀測控制算法中多個變量，如擺桿角度，方向，電機速度以與加速度等等。通過撥碼開關(guān)來決定運行不同的程序，以實現(xiàn)不同的功能。相互功能之間沒有干擾。實現(xiàn)另一個功能需要單片機復(fù)位后重新運行。記錄下每次實現(xiàn)不同功能的性能指標(biāo)。4.2 測試條件與儀器測試條件：支架水平放置于桌面上，

24、保證擺桿處于自然下垂?fàn)顟B(tài)，除電機外無其他外力對擺桿造成干擾（空氣阻力，摩擦力等不算在）。測試儀器：直流穩(wěn)壓電源，數(shù)字示波器，數(shù)字萬用表，秒表，量角器.4.3 測試結(jié)果與分析4.3.1測試結(jié)果(數(shù)據(jù))測試功能一：擺桿起振測試次數(shù)123456擺動幅度（度） 6060 60606060擺動時間（s）212354測試功能二：擺桿完成圓周運動說明：以完整的完成一個圓周運動的時間為計時標(biāo)準(zhǔn)。測試次數(shù)12345678完成時間（s）2222121測試功能三：擺桿盡快達到倒立平衡狀態(tài)并保持5s以上，且旋轉(zhuǎn)臂旋轉(zhuǎn)的轉(zhuǎn)動角度不大于90度。測試次數(shù)12345678保持時間（s）555555旋轉(zhuǎn)臂最大轉(zhuǎn)動角 （度）90

25、90909090測試功能四：從擺桿處于自然下垂?fàn)顟B(tài)開始，控制旋轉(zhuǎn)臂做往復(fù)旋轉(zhuǎn)運動，盡快使擺桿擺起倒立，保持倒立時間不小于10s測試次數(shù)12345678擺起時間（s）23-12.52514保持時間（s）101001010101010測試功能五：在擺桿保持倒立狀態(tài)下，施加干擾后擺桿能繼續(xù)保持倒立或2s恢復(fù)倒立狀態(tài)。測試次數(shù)12345678能否保持能能能能能能能否恢復(fù)時間（s）23433454測試功能六：在擺桿保持倒立狀態(tài)的前提下，旋轉(zhuǎn)臂做圓周運動，并盡快使單方向轉(zhuǎn)過角度達到或超過360度。測試次數(shù)12345678旋轉(zhuǎn)臂轉(zhuǎn)過360度時間（s）555555554.3.2測試分析與結(jié)論根據(jù)上述測試數(shù)據(jù)，

26、該系統(tǒng)基本上能夠達到基本部分和發(fā)揮部分(1)(3)的性能指標(biāo)，由此可以得出以下結(jié)論：1、電機的性能不是很理想，聯(lián)軸器有些松動，長時間運行兩者有些脫節(jié)，起振達到平衡很困難，2、雙環(huán)控制需要時間上的配合，加入的時間和參數(shù)很重要。3、好的硬件電路時成功的奠基石。綜上所述，本設(shè)計基本達到設(shè)計要求。5總結(jié)本次比賽中我們組遇到的最大的困難是硬件問題，包括合適的電動機的選型，先后換了三種型號的電動機，電動機與底座和旋轉(zhuǎn)臂的連接，以與能夠經(jīng)受住長時間離心力作用的聯(lián)軸器和鋼珠等支承結(jié)構(gòu)。四天三夜中很多時間在組建和檢修硬件部分，導(dǎo)致浪費了很多調(diào)試程序的時間，東奔西跑地尋找合適的材料也浪費了很多體力。在斷斷續(xù)續(xù)的將

27、硬件做好之后，在調(diào)試軟件時我們一直采用的程序設(shè)計思路在我們攻克倒立平衡的穩(wěn)定性時，遇到了很大的挑戰(zhàn)，我們在這個程序思路上下了很多功夫，但是調(diào)整結(jié)果很不滿意。在第四天的下午，離比賽結(jié)束還有幾個小時時，我們放棄了一直堅持的程序主線將加速度環(huán)作為環(huán)，速度環(huán)作為外環(huán)進行控制，轉(zhuǎn)而采用了將速度環(huán)作為環(huán)，而加速度環(huán)作為外環(huán)，消除了速度的采樣滯后，才取得了比較可喜的進步，但是由于時間實在太有限，我們沒有機會將所有部分都完美的移植在新的程序思路上。遺憾是有的，但是收獲也是巨大的，四天三夜，體驗到了很多東西，團隊的力量，溝通的重要性，最重要的是遇到困難能夠不被得失的情緒所干擾，而是一心一意去解決問題，這是我們以

28、后對待任何困難的一種成功的心態(tài)。在這比賽的最后，要感學(xué)校和學(xué)院老師的給力的后勤工作，使我們能夠在不被煩擾的情況下，用最充足的時間完成比賽。感全國大學(xué)生電子設(shè)計大賽的組委會，提供這樣優(yōu)秀的平臺和機會給我們充足的鍛煉。附件1：系統(tǒng)電路原理圖P0P0P1P1P2P2P3P3J0J0J1J1K0K0K1K1K2K2K3K3T0T0T1T1T2T2T3T3T4T4T5T5T6T6T7T7K4K4K5K5B0B0B1B1B2B2B3B3B4B4B5B5B6B6B7B7P7P7P6P6P5P5P4P4K7K7NCNCM7M7M6M6M5M5M4M4M3M3M2M2M1M1M0M0J7J7J6J6S7S7S6

29、S6S5S5S4S4S3S3S2S2S1S1S0S0+5+5+5+5GNDGNDGNDGNDAD15AD15AD14AD14AD13AD13AD12AD12AD11AD11AD10AD10AD9AD9AD8AD8AD7AD7AD6AD6AD5AD5AD4AD4AD3AD3AD2AD2AD1AD1AD0AD0A7A7A6A6A5A5A4A4A3A3A2A2A1A1A0A0VRHVRHGNDGNDGNDGND+5+5H7H7H6H6H5H5H4H4E7E7E6E6E5E5E4E4H3H3H2H2H1H1H0H0E3E3E2E2E1E1E0E09S12XS/E-P11212345678S1X012X

30、114X215X311Y01Y15Y22Y34INH6A10B9VEE7X13Y3U2CD4052R120kR220kR320kR420k+5GND1VIN+5ON/OFF3GND4FEEDBACK2OUTPUTLM2596U2+C1680UFD1IN5824L33u+C2220UF+12V-12V+5VGNDVR?VRIN15IN27IN310IN412ENA6ENB11VSS9VS4OUT12OUT23OUT31314OUT4ISEN A1ISEN B15U3L298ND1D2D3D4D5D6D7D8+12VGNDGNDGND+C3100UFC40.1UFC60.1UF+C5100UF+5

31、V+5V1234J1CON41IRd/21D31CP4ISd51Q61Q/7GND14VSS132Rd/122D112CP102Sd92Q82Q/*Component_1P1P51NC2VCC3GND4NC5A/6A7B/8B9I/10I11892+5v3GND419251936P771958P6919710P4U521B31Y42A52B62Y7GND11A14VCC134B124A114Y103B93A83YU674LS86附件2：電機控制源程序子函數(shù)：#include carsub.h#include #include derivative.hvoid GetMotorPulse(voi

32、d) unsigned int nLeftPulse,nRightPulse; unsigned char leftdir,rightdir; /unsigned char ii; leftdir = PORTB & 0 x01; /讀取轉(zhuǎn)向 rightdir = PORTB & 0 x04; nLeftPulse = PACNT; /left為部計數(shù)器，right為外部計數(shù)器 nRightPulse = PORTA; /讀取脈沖數(shù) PACNT = 0; PTJ=PTJ|0XFF; Delay(15); PTJ=PTJ&0X00; /計數(shù)器清零 if(leftdir = 0) LeftMoto

33、rPulse -= (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;

34、void SetMotorVoltage(int nLeftVal) if(nLeftValMOTOR_OUT_MAX) nLeftVal=MOTOR_OUT_MAX;if(nLeftVal 0) PWMDTY45 = 0; PWMDTY01 = nLeftVal + 120; else PWMDTY45 = -nLeftVal + 120; PWMDTY01 = 0; void MotorOutput(void) /計算最終給定的PWM占空比/int LeftMotorOutput;/float K,P;/if (PORTB_PB3 = 1)/ K = 350;/else K = 700;L

35、eftMotorOutput = (int)K*PositionControlOut + SpeedControlOut; if (PORTB_PB4 =0) if (LeftMotorSpeed 0) g_ucRight = 1; g_ucLeft = 0; if (g_ucRight = 1)&(GravityAngle 0)&(GravityAngle = 0)&(LeftMotorSpeedOld 0) g_ucLeft = 1; g_ucRight = 0; if (g_ucLeft = 1)&(GravityAngle -60) LeftMotorOutput = 0; if (P

36、ORTB_PB3 = 0) if (LeftMotorSpeed 0) g_ucRight = 1; g_ucLeft = 0; if (g_ucRight = 1)&(GravityAngle =0)&(GravityAngle = 0)&(LeftMotorSpeedOld 0) g_ucLeft = 1; g_ucRight = 0; if (g_ucLeft = 1)&(GravityAngle = -180) LeftMotorOutput = -LeftMotorOutput; SetMotorVoltage(LeftMotorOutput); void PositionContr

37、ol(void) float fP,fD; POSITION_CONTROL_P = 12; POSITION_CONTROL_D = 0.002; if(GravityAngle -45 ) fP = -15; /if(GravityAngle 180) /if(GravityAngle 315) / fP = -15; /else /fP = POSITION_CONTROL_P*tancodeGravityAngle - 180; / else /if(GravityAngle 180) GravityAngle = GravityAngle - 360; AngleSpeed = AN

38、GLE_SPEED_RATIO*LeftMotorSpeed; void Delay(int num) int delay_c; for(delay_c=0;delay_cnum;delay_c+); void PositionSetOut(void) float fvalue; fvalue = PositionControlNew - PositionControlOld; PositionControlOut = fvalue*(PositionControlCount + 1)/POSITION_CONTROL_COUNT + PositionControlOld; 主函數(shù)：#incl

39、ude #include derivative.h #include init.h #include carsub.hconst float tancode181= 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

40、,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.

41、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

42、.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.

43、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 ; /const floatfloat POSITION_CONTROL_

44、P,POSITION_CONTROL_D,POSITION_CONTROL_I;float SPEED_CONTROL_P,SPEED_CONTROL_I,SPEED_CONTROL_D;float GRAVITY_ANGLE_RATIO = 0.5,ANGLE_SPEED_RATIO = 100;float K=650; unsigned char g_ucBegin,g_ucRight,g_ucLeft; unsigned char PositionControlCount;int LeftMotorPulse;/int g_iZOffset; int g_lPosition = 0,g_lPositionBase = 0; /long g_lPositionGoal;/float Posit