畢業(yè)設(shè)計(jì)(論文)風(fēng)力擺控制系統(tǒng)

1、<p>　　武 昌 理 工 學(xué) 院</p><p>　　本科畢業(yè)設(shè)計(jì)(論文)</p><p>　　題　　目： 風(fēng)力擺控制系統(tǒng)</p><p>　　學(xué)　　院: 信息工程學(xué)院</p><p>　　專　　業(yè)： 自 動(dòng) 化</p><p>　　學(xué)　　號(hào)： </p><

2、;p>　　學(xué)生姓名： </p><p>　　指導(dǎo)教師： </p><p>　　二○一六 年 四月 八 日</p><p><b>　　風(fēng)力擺控制系統(tǒng)</b></p><p><b>　　摘要</b></p><p>　　本系統(tǒng)由微處理器STM

3、32，直流風(fēng)機(jī)及驅(qū)動(dòng)，慣性測(cè)量單元MPU6050，OLED顯示，4X4矩陣鍵盤，蜂鳴器構(gòu)成。系統(tǒng)運(yùn)行可分為兩部分：風(fēng)力擺的控制部分和預(yù)設(shè)值輸入與顯示部分。風(fēng)力擺控制是由STM32定時(shí)器輸出頻率，占空比可調(diào)的PWM波經(jīng)驅(qū)動(dòng)模塊之后，使直流風(fēng)機(jī)的轉(zhuǎn)數(shù)與PWM波占空比成線性關(guān)系，以此達(dá)到控制風(fēng)力擺擺動(dòng)的目的。預(yù)設(shè)值輸入與顯示是通過(guò)矩陣鍵盤將預(yù)設(shè)值輸入到微處理器。再由處理器將數(shù)據(jù)送至OLED上顯示。MPU6050的三軸加速度數(shù)據(jù)和三軸角速度數(shù)據(jù)

4、，經(jīng)MCU解算之后得到風(fēng)力擺擺角，形成信號(hào)反饋回路。組成閉環(huán)控制系統(tǒng)。控制算法為PI調(diào)節(jié)。</p><p>　　關(guān)鍵詞：STM32F103VCT6； MPU6050；PID控制算法。</p><p>　　Wind pendulum control system</p><p><b>　　Abstract</b></p><

5、p>　　This system is structured by microprocessor STM32, DC fan and its drive, inertial Measuring Unit MPU60510, OLED display, 4*4 rectangular keyboard, and buzzer. The operation of this system is associated with two ma

6、in parts, one is wind pendulum control, and another one is default input and output. The frequency of STM32 timer andthe adjustable duty ratio of PWM go through the wave driving template make the turns of rotation of DC

7、fan are linear with duty ratio of PWM, so thatthe wind pendulum is </p><p>　　Key word: STM32F103VCT6; MPU6050;PID</p><p><b>　　目錄</b></p><p><b>　　前言1</b><

8、/p><p>　　第一章 系統(tǒng)方案設(shè)計(jì)與論證3</p><p>　　1.1 系統(tǒng)整體結(jié)構(gòu)3</p><p>　　1.2 方案簡(jiǎn)述3</p><p>　　1.3 電源選取3</p><p>　　1.4 直流風(fēng)機(jī)3</p><p>　　1.5 三維角度傳感器3</p><

9、;p>　　1.6 風(fēng)機(jī)驅(qū)動(dòng)模塊4</p><p>　　1.7 風(fēng)力擺結(jié)構(gòu)4</p><p>　　1.8 控制算法4</p><p>　　第二章 理論分析與計(jì)算5</p><p>　　2.1 起擺分析5</p><p>　　2.2 擺動(dòng)固定長(zhǎng)度5</p><p>　　2.3

10、PID算法分析5</p><p>　　2.4 擺角解算6</p><p>　　第三章 系統(tǒng)各模塊電路設(shè)計(jì)9</p><p>　　3.1 穩(wěn)壓電路模塊9</p><p>　　3.2 矩陣鍵盤模塊9</p><p>　　3.3 風(fēng)機(jī)驅(qū)動(dòng)模塊10</p><p>　　第四章 軟件設(shè)計(jì)流

11、程11</p><p>　　4.1 軟件結(jié)構(gòu)11</p><p>　　4.2 軟件流程簡(jiǎn)介11</p><p>　　4.3 顯示模塊初始化及其界面程序分析11</p><p>　　4.4 MPU6050的初始化及相關(guān)程序段分析15</p><p>　　第五章 測(cè)試方案與測(cè)試結(jié)果18</p>

12、<p>　　5.1 測(cè)試所用儀器18</p><p>　　5.2 測(cè)試方案18</p><p>　　5.3 誤差分析19</p><p><b>　　致謝20</b></p><p><b>　　參考文獻(xiàn)21</b></p><p><b>　

13、　附錄22</b></p><p><b>　　前言</b></p><p>　　風(fēng)力擺控制系統(tǒng)即控制做自由擺運(yùn)動(dòng)。自由擺是一種典型而具有代表性的運(yùn)動(dòng)形式，一直以來(lái)人們對(duì)單擺運(yùn)動(dòng)的運(yùn)動(dòng)規(guī)律進(jìn)行了大量的研究，自由擺運(yùn)行相當(dāng)于是從單擺運(yùn)動(dòng)衍生的運(yùn)動(dòng)形式。風(fēng)力擺系統(tǒng)是一個(gè)典型非線性，不穩(wěn)定的系統(tǒng)。在經(jīng)典控制理論和現(xiàn)代控制理論的應(yīng)用是一個(gè)很具有代表性的問(wèn)題。將其

14、作為實(shí)驗(yàn)裝置時(shí)可以測(cè)試控制理論，檢驗(yàn)控制方法和思路。這些控制思想都能廣泛應(yīng)用于一般工業(yè)過(guò)程控制，所以對(duì)于風(fēng)力擺控制系統(tǒng)在控制理論的探索方面具有很重要的理論研究和實(shí)際應(yīng)用價(jià)值。</p><p>　　該系統(tǒng)所運(yùn)用的傳感器為慣性測(cè)量單元，慣性技術(shù)作為一門高科技尖端技術(shù)，它結(jié)合了機(jī)電、光學(xué)、數(shù)學(xué)、力學(xué)、控制及計(jì)算機(jī)科學(xué)的尖端技術(shù)。慣性是所有質(zhì)量體的基本屬性，建立在慣性原理基礎(chǔ)上的系統(tǒng)不需要任何外界信息，也不會(huì)向外輻射任何

15、信息，僅靠其系統(tǒng)本身就能在全天候環(huán)境下，自主的進(jìn)行連續(xù)的三維定向和定位。并且能獲取運(yùn)載體完備的運(yùn)動(dòng)信息，以及具有優(yōu)秀的隱蔽性。這些特點(diǎn)是其他導(dǎo)航系統(tǒng)所無(wú)法比擬的。</p><p>　　運(yùn)動(dòng)控制系統(tǒng)包括的處理運(yùn)動(dòng)算法和信號(hào)的控制器，能增強(qiáng)信號(hào)、可供應(yīng)執(zhí)行器提供運(yùn)動(dòng)輸出放大器；慣性測(cè)量單元檢測(cè)數(shù)據(jù)進(jìn)行反饋，可基于輸出和輸入的比較值，構(gòu)成閉環(huán)系統(tǒng)。調(diào)節(jié)器對(duì)偏差信號(hào)進(jìn)行比例、積分、微分運(yùn)算。微分作用加快系統(tǒng)動(dòng)作速度，減少

16、超調(diào)，克服振蕩；積分作用消除靜態(tài)誤差。三種作用方式結(jié)合在一起可以達(dá)到既快速便捷，平穩(wěn)準(zhǔn)確的效果。采用這種方式構(gòu)成的調(diào)節(jié)器，只需改變輸入和反饋電路的內(nèi)容，便能獲得不同的調(diào)節(jié)規(guī)律。</p><p>　　控制系統(tǒng)里的一些比較抽象的概念，反饋、穩(wěn)定性、發(fā)散收斂速度和系統(tǒng)的抗干擾性能等，風(fēng)力擺都能有很直觀的表現(xiàn)。對(duì)于該系統(tǒng)典型的控制方法有，PID控制、智能控制、模糊控制等?？刂评碚摰难芯咳藛T將風(fēng)力擺系統(tǒng)作為研究對(duì)象，希望開(kāi)

17、拓出新的控制方法，應(yīng)用在航空航天等高科技領(lǐng)域。</p><p>　　傳統(tǒng)8/16位單片機(jī)，雖然在工業(yè)領(lǐng)域中得到了廣泛運(yùn)用，但是現(xiàn)在的越來(lái)越多的工程控制產(chǎn)品的需求多功能、易用界面、低功耗、多任務(wù)等等一系列任務(wù)。對(duì)于這些需求，僅僅使用傳統(tǒng)8/16位單片機(jī)已經(jīng)不足以滿足。看準(zhǔn)了這個(gè)市場(chǎng)，ARM公司推出了Cortex</p><p>　　-M3處理器，它對(duì)微控制器和低成本應(yīng)用提出優(yōu)化，具有低成本、

18、低功耗和高性能的特點(diǎn)，能夠滿足工程師進(jìn)行創(chuàng)新設(shè)計(jì)的需求。</p><p>　　意法半導(dǎo)體公司推出了基于Cortex-M3內(nèi)核32位的微控制器STM32,相對(duì)于ARM7、ARM9這類速度更快的32位來(lái)說(shuō)，采用最合適的內(nèi)核構(gòu)架以及獨(dú)特的開(kāi)發(fā)方式。它具有性價(jià)比高和易于開(kāi)發(fā)兩個(gè)優(yōu)勢(shì)。這導(dǎo)致STM32一上市就以非?？斓乃俣日紦?jù)了中低端微控制器的市場(chǎng)。</p><p>　　STM32的有兩種開(kāi)發(fā)方式。

19、一種是與傳統(tǒng)單片機(jī)開(kāi)發(fā)方式相同的寄存器開(kāi)發(fā)，就是通過(guò)對(duì)微處理器的進(jìn)行直接的讀寫(xiě)操作。使用這種方式需要對(duì)微處理器的底層寄存器以及外圍電路有很深的理解。而且由于STM32的寄存器種類和數(shù)量都要比傳統(tǒng)單片機(jī)多得多，故使用寄存器開(kāi)發(fā)方式有比較高的門檻。這種方式的優(yōu)點(diǎn)也十分明顯，它十分貼近物理電路層面，這樣設(shè)計(jì)出的系統(tǒng)，其穩(wěn)定性、可靠性以及運(yùn)行速度都要好得多。</p><p>　　另外一種開(kāi)發(fā)方式是庫(kù)函數(shù)開(kāi)發(fā)。這是一種自頂

20、而下的方式剛好與前一種方式相反，可以從上層的API一層一層跟蹤到底層寄存器，從而徹徹底底的了解寄存器，了解CPU的內(nèi)存分布，再到啟動(dòng)代碼、開(kāi)發(fā)環(huán)境設(shè)置等等。這種方式大大的降低了學(xué)習(xí)開(kāi)發(fā)STM32的門檻，讓初學(xué)者能夠快快速上手，僅通過(guò)調(diào)用庫(kù)里的API（應(yīng)用程序接口）就能夠?qū)崿F(xiàn)一個(gè)大型程序的搭建。做出能滿足各種用戶需求的應(yīng)用。</p><p>　　第一章 系統(tǒng)方案設(shè)計(jì)與論證</p><p>

21、　　1.1 系統(tǒng)整體結(jié)構(gòu)</p><p><b>　　圖1 系統(tǒng)整體框圖</b></p><p><b>　　1.2 方案簡(jiǎn)述</b></p><p>　　本系統(tǒng)以超低功耗MCUSTM32F103VCT6為控制核心，風(fēng)力擺采用萬(wàn)向節(jié)、碳素纖維管、四個(gè)直流風(fēng)機(jī)構(gòu)成。MCU輸出PWM波經(jīng)過(guò)直流電機(jī)驅(qū)動(dòng)控制四個(gè)風(fēng)機(jī)轉(zhuǎn)動(dòng)，使風(fēng)力擺

22、獲得擺動(dòng)動(dòng)力。MPU6050的三軸加速度數(shù)據(jù)和三軸角速度數(shù)據(jù)，經(jīng)MCU解算之后得到風(fēng)力擺擺角，形成信號(hào)反饋回路。組成閉環(huán)控制系統(tǒng)。</p><p><b>　　1.3 電源選取</b></p><p>　　自制穩(wěn)壓源，電源電壓穩(wěn)定，上電即可用，能省掉系統(tǒng)校準(zhǔn)的麻煩，自制穩(wěn)壓源采用78系列穩(wěn)壓芯片，最大輸出電流為3A，但對(duì)該項(xiàng)目而言，4個(gè)直流風(fēng)機(jī)轉(zhuǎn)動(dòng)時(shí)，電流過(guò)大。自制穩(wěn)

23、壓源并不能滿足要求。采用兆信KXN3030B直流穩(wěn)壓電源，最大電壓可達(dá)到30V，最大輸出電流能到達(dá)30A，能提供四個(gè)風(fēng)機(jī)轉(zhuǎn)動(dòng)所需電流，電壓穩(wěn)定性也可滿足要求。</p><p><b>　　1.4 直流風(fēng)機(jī)</b></p><p>　　采用通用小型軸流風(fēng)機(jī)，12V直流供電，輕巧。但功率不大，風(fēng)力太小，并不能讓風(fēng)力擺在短時(shí)間內(nèi)達(dá)到題目所需要的擺動(dòng)長(zhǎng)度。采用小型涵道有刷風(fēng)機(jī)

24、，涵道風(fēng)機(jī)屬于高速軸流風(fēng)機(jī)， 12V直流供電，風(fēng)機(jī)產(chǎn)生風(fēng)力能有效使風(fēng)力擺擺動(dòng)。能滿足要求。</p><p>　　1.5 三維角度傳感器</p><p>　　采用L3GD20三軸陀螺儀，能測(cè)量三軸角速度。如果在此使用還需再另外加載一個(gè)三軸加速度計(jì)，而采用微處理單元MPU6050。該芯片包含三軸加速度計(jì)及三軸陀螺儀，外圍電路設(shè)計(jì)簡(jiǎn)單，采用IIC通信協(xié)議，具有量程大，精度高，功耗低等特點(diǎn)。完全滿

25、足題目要求且具備更優(yōu)的性能。</p><p>　　1.6 風(fēng)機(jī)驅(qū)動(dòng)模塊</p><p>　　采用L298N電機(jī)驅(qū)動(dòng)模塊，最大工作電壓為50V，最高電流4A，一個(gè)芯片能驅(qū)動(dòng)兩個(gè)直流電機(jī)。對(duì)于本系統(tǒng)來(lái)說(shuō)，4A電流供兩個(gè)電機(jī)工作有點(diǎn)勉強(qiáng)，調(diào)節(jié)裕度太小。而采用BTN7971的驅(qū)動(dòng)模塊，驅(qū)動(dòng)板工作電壓范圍：7V~14V，最大電流能達(dá)到60A，能滿足題目要求。且調(diào)節(jié)裕度很大。</p>&

26、lt;p><b>　　1.7 風(fēng)力擺結(jié)構(gòu)</b></p><p>　　一組4個(gè)直流風(fēng)機(jī)用粗單股導(dǎo)線吊掛在支架上，擺動(dòng)阻力小，受力更均勻，但單股導(dǎo)線在畫(huà)圓時(shí)必然產(chǎn)生自旋，加大了系統(tǒng)的控制難度。如果用一組4個(gè)直流風(fēng)機(jī)用碳素纖維管通過(guò)萬(wàn)向節(jié)固定在支架上，由于碳棒和萬(wàn)向節(jié)是剛體，在控制過(guò)程中不會(huì)遇到自旋的問(wèn)題，萬(wàn)向節(jié)擺動(dòng)時(shí)阻力只比粗導(dǎo)線略大，對(duì)擺動(dòng)影響不大。</p><p&

27、gt;<b>　　1.8 控制算法</b></p><p>　　采用模糊控制算法，模糊控制算法是對(duì)手動(dòng)操作者的手動(dòng)控制策略、經(jīng)驗(yàn)的總結(jié).模糊控制算法有多種實(shí)現(xiàn)形式,應(yīng)用最早、最廣泛的是查表法,查表法可大大提高模糊控制的時(shí)效性,節(jié)省內(nèi)存,它不需要事先知道對(duì)象的數(shù)學(xué)模型,具有系統(tǒng)響應(yīng)快、超調(diào)小、過(guò)渡過(guò)程時(shí)間短等優(yōu)點(diǎn)，但編程復(fù)雜，數(shù)據(jù)處理量大。 如果采用PID算法，按比例（P）、積分（I）、微分（

28、D）的函數(shù)關(guān)系,進(jìn)行運(yùn)算,將其運(yùn)算結(jié)果用以輸出控制。優(yōu)點(diǎn)是控制精度高,且算法簡(jiǎn)單明了。對(duì)于本系統(tǒng)的控制已足夠精確，節(jié)約了MCU的資源和運(yùn)算時(shí)間。 </p><p>　　第二章 理論分析與計(jì)算</p><p><b>　　2.1 起擺分析</b></p><p>　　風(fēng)力擺擺動(dòng)是依靠風(fēng)機(jī)轉(zhuǎn)動(dòng)推動(dòng)空氣，空氣對(duì)風(fēng)力擺有一個(gè)反作用力使風(fēng)力擺運(yùn)動(dòng)起來(lái)，

29、想讓風(fēng)力擺擺動(dòng)起來(lái)，就要使空氣對(duì)風(fēng)力擺的作用力力的方向與速度方向一致。只要控制風(fēng)機(jī)，滿足這個(gè)條件轉(zhuǎn)動(dòng)，就可以使風(fēng)力擺的擺動(dòng)幅度越來(lái)越大。本題我們是通過(guò)一開(kāi)始讓一個(gè)風(fēng)機(jī)機(jī)轉(zhuǎn)動(dòng)一定時(shí)間，形成小幅度擺動(dòng)，然后在擺動(dòng)到最高點(diǎn)的時(shí)候，每次位于最高點(diǎn)的風(fēng)機(jī)轉(zhuǎn)動(dòng)，這樣使擺動(dòng)幅度慢慢增加。</p><p>　　2.2 擺動(dòng)固定長(zhǎng)度</p><p>　　風(fēng)力擺擺動(dòng)過(guò)程就是動(dòng)能與重力勢(shì)能相互轉(zhuǎn)化的過(guò)程。理想情

30、況下，風(fēng)力擺應(yīng)該是可以一直擺動(dòng)不停止。既只要彌補(bǔ)掉風(fēng)力擺每次擺動(dòng)損失的能量就能讓擺擺長(zhǎng)穩(wěn)定不變。通過(guò)調(diào)節(jié)風(fēng)機(jī)轉(zhuǎn)動(dòng)，能做到使擺長(zhǎng)無(wú)明顯衰減也不明顯增大，再通過(guò)MPU6050數(shù)據(jù)，姿態(tài)解算得到擺角，每一個(gè)擺角都能對(duì)應(yīng)成相應(yīng)擺長(zhǎng)。這樣就可以得到與設(shè)定偏差長(zhǎng)度，再使用PID控制算法自動(dòng)調(diào)節(jié)減小偏差，達(dá)到擺動(dòng)固定長(zhǎng)度的要求。</p><p>　　2.3 PID算法分析</p><p>　　工程實(shí)際

31、中，應(yīng)用最為廣泛調(diào)節(jié)器控制規(guī)律為比例、積分、微分控制，簡(jiǎn)稱PID控制。 PID控制器問(wèn)世至今已有近70年歷史，它以其結(jié)構(gòu)簡(jiǎn)單、穩(wěn)定性好、工作可靠、調(diào)整方便而成為工業(yè)控制主要技術(shù)之一。當(dāng)被控對(duì)象結(jié)構(gòu)和參數(shù)不能完全掌握，或不到精確數(shù)學(xué)模型時(shí)，控制理論其它技術(shù)難以采用時(shí)，系統(tǒng)控制器結(jié)構(gòu)和參數(shù)必須依靠經(jīng)驗(yàn)和現(xiàn)場(chǎng)調(diào)試來(lái)確定。</p><p>　　PID連續(xù)系統(tǒng)模型為：</p><p>　　MCU是數(shù)

32、字調(diào)節(jié)，動(dòng)作是斷續(xù)的。它以采樣周期為間隔，對(duì)MPU6050輸出進(jìn)行采樣，由于該傳感器輸出為數(shù)字量，故不需要進(jìn)行模數(shù)轉(zhuǎn)換。采用數(shù)據(jù)經(jīng)過(guò)解算后的到偏差信號(hào)，經(jīng)過(guò)一定調(diào)節(jié)規(guī)律算出輸出值，再通過(guò)PWM波向傳送給風(fēng)機(jī)的驅(qū)動(dòng)電路。對(duì)MCU來(lái)說(shuō)，需要將上式離散化才能進(jìn)行運(yùn)用。</p><p><b>　　PID離散模型為:</b></p><p>　　再將離散算法再是通過(guò)軟件實(shí)現(xiàn)：

33、</p><p>　　X=X.in - X.offset;</p><p>　　increment += X;//對(duì)X進(jìn)行積分</p><p>　　if(increment > increment.max)</p><p>　　increment = increment.max;</p><p>　　el

34、se if(increment < -increment.max)</p><p>　　increment = -increment.max;</p><p>　　pid_out = kp * X + ki * increment + kd * (X - X _old);</p><p>　　X _old = X; </p><p>　

35、　其中X為偏差信號(hào)、X.in為傳感器信號(hào)輸入、X.offset為設(shè)定值、increment偏差信號(hào)積分，increment.max 為積分上限，kp、ki、kd為PID參數(shù)，整個(gè)程序每隔時(shí)間運(yùn)行一次。</p><p><b>　　2.4 擺角解算</b></p><p>　　系統(tǒng)的由于慣性測(cè)量單元（MPU6050）直接固聯(lián)在單擺中心軸線上，是一個(gè)捷聯(lián)式慣導(dǎo)系統(tǒng)。MPU

36、6050測(cè)量擺錘的角運(yùn)動(dòng)信息和線運(yùn)動(dòng)信息，MCU根據(jù)這些信息可解算出擺角。</p><p>　　以下參數(shù)均為MPU6050的原始數(shù)據(jù)經(jīng)濾波之后的得到：</p><p>　　參數(shù)，，分別對(duì)應(yīng)三個(gè)軸的角速度數(shù)據(jù)，單位是 弧度/秒；</p><p>　　參數(shù)，，分別對(duì)應(yīng)三個(gè)軸的加速度數(shù)據(jù)，單位是 米/秒；</p><p>　　先根據(jù)加速度數(shù)據(jù)轉(zhuǎn)換單

37、位加速度分別為，，</p><p>　　設(shè)將地理坐標(biāo)參考系轉(zhuǎn)到當(dāng)前的系統(tǒng)參考系（角速度積分）的旋轉(zhuǎn)用四元數(shù)表示為：</p><p>　　將地理重力單位向量轉(zhuǎn)到當(dāng)前參考系。則有：</p><p>　　向量（，，）為在當(dāng)前參考坐標(biāo)系的中地理重力單位向量。</p><p>　　MPU6050的加速度計(jì)由于噪聲比較大，受振動(dòng)影響比其角速度計(jì)明顯，在一

38、段較短時(shí)間內(nèi)的可靠性不高。而角速度計(jì)噪聲小，但是由于積分是離散的，長(zhǎng)時(shí)間的積分會(huì)出現(xiàn)漂移，從而產(chǎn)生較大誤差。因此需要用加速度計(jì)求得的姿態(tài)來(lái)矯正角速度積分得到的姿態(tài)。</p><p>　　在當(dāng)前參照系上，由于擺錘只受重力作用，故加速度計(jì)輸出即為測(cè)出來(lái)的重力向量是（，，）；根據(jù)陀螺儀工作原理，角速度積分后的姿態(tài)來(lái)推算出的重力向量是（，，），它們之間的誤差向量，就是角速度積分后的姿態(tài)和加速度計(jì)測(cè)出來(lái)的姿態(tài)之間的誤差。&

39、lt;/p><p>　　向量間的誤差即向量積(也叫外積、叉乘)：</p><p>　　、、 就是兩個(gè)重力向量的叉積。這個(gè)叉積向量依然是位于當(dāng)前坐標(biāo)參考系上，而角速度積分誤差也是在當(dāng)前坐標(biāo)參考系，而且叉積的大小與角速度積分誤差成正比，正好拿來(lái)糾正陀螺。對(duì)叉積進(jìn)行積分：</p><p>　　用叉乘誤差來(lái)做PI修正陀螺零偏，通過(guò)調(diào)節(jié)Kp，Ki兩個(gè)參數(shù)，可以控制加速度計(jì)修正陀螺

40、儀積分姿態(tài)的速度</p><p>　　再對(duì)角速度進(jìn)行直接積分：</p><p>　　再將該四元數(shù)歸一化之后，轉(zhuǎn)換成歐拉角即可得到兩個(gè)方向的擺角</p><p>　　第三章 系統(tǒng)各模塊電路設(shè)計(jì)</p><p>　　3.1 穩(wěn)壓電路模塊</p><p>　　由于直流電機(jī)做執(zhí)行機(jī)構(gòu)，所以需要有給直流電機(jī)供電的電源，并且為了

41、防止電機(jī)轉(zhuǎn)動(dòng)帶來(lái)的電流對(duì)MCU的影響，MCU供電和電機(jī)供電要分開(kāi)。所以選用7805穩(wěn)壓芯片輸出5V穩(wěn)定電壓給MCU供電。C1、C2分別為輸入端和輸出端濾波電容。</p><p>　　圖2 穩(wěn)壓電路模塊原理圖</p><p>　　3.2 矩陣鍵盤模塊</p><p>　　鍵盤作為人機(jī)交互的輸入設(shè)備，其在要求有充足的輸入端為人機(jī)交互信息輸入做好硬件基礎(chǔ)，CH451。內(nèi)

42、置RC 振蕩電路，可以動(dòng)態(tài)驅(qū)動(dòng)8 位數(shù)碼管或者64 位LED燈，具有BCD 譯碼、移位等功能；同時(shí)還可以進(jìn)行64 鍵的鍵盤掃描；CH451 通過(guò)1 線或者可以級(jí)聯(lián)的4 線串行接口與MCU等交換數(shù)據(jù)；并且提供上電復(fù)位和看門狗等監(jiān)控功能。本系統(tǒng)以CH451為核心的矩陣鍵盤，CH451芯片內(nèi)部自帶硬件消抖，大大方便軟件處理，并可做到多達(dá)64個(gè)按鍵，本次設(shè)計(jì)使用16個(gè)按鍵。</p><p>　　圖3 CH451矩陣鍵盤

43、原理圖</p><p>　　3.3 風(fēng)機(jī)驅(qū)動(dòng)模塊</p><p>　　風(fēng)機(jī)作為本系統(tǒng)的動(dòng)力部分，其功率要求大，對(duì)驅(qū)動(dòng)承受電流值要求高，以BTN7971B驅(qū)動(dòng)芯片為核心的電機(jī)驅(qū)動(dòng)，可以承受0—68A電流，本系統(tǒng)每個(gè)風(fēng)機(jī)驅(qū)動(dòng)采用雙BTN7971B大電流H橋驅(qū)動(dòng)。</p><p>　　圖4 風(fēng)機(jī)驅(qū)動(dòng)模塊原理圖</p><p>　　第四章 軟件設(shè)

44、計(jì)流程</p><p><b>　　4.1 軟件結(jié)構(gòu)</b></p><p>　　圖5 軟件流程圖</p><p>　　4.2 軟件流程簡(jiǎn)介</p><p>　　開(kāi)始運(yùn)行以后，首先進(jìn)行系統(tǒng)各個(gè)硬件的初始化，然后進(jìn)入循環(huán)體進(jìn)行按鍵掃描，檢測(cè)到有鍵按下通過(guò)switch判斷后進(jìn)入相應(yīng)的功能界面中，進(jìn)入界面后輸入相應(yīng)的參數(shù)后

45、，按下確定鍵，風(fēng)力擺即可安裝設(shè)定參數(shù)進(jìn)行動(dòng)作。</p><p>　　4.3 顯示模塊初始化及其界面程序分析</p><p>　　本系統(tǒng)使用的顯示模塊為ALIENTEK 推出的一款高性能 OLED 顯示模塊ATK-0.96’ OLED，要使用這個(gè)顯示模塊，主要有三個(gè)步驟，首先是選擇MCU與OLED通信所使用的通信協(xié)議，再根據(jù)所選的通信協(xié)議進(jìn)行物理線路的連接，最后用MCU通過(guò)協(xié)議對(duì)模塊寄存器

46、進(jìn)行配置。</p><p>　　這里使用的是SPI通信協(xié)議；</p><p>　　SPI, Serial Perripheral Interface, 串行外圍設(shè)備接口, 是 Motorola 公司推出的一種同步串行接口技術(shù). SPI 總線在物理上是通過(guò)接在外圍設(shè)備微控制器(PICmicro) 上面的微處理控制單元 (MCU) 上叫作同步串行端口(Synchronous Serial Po

47、rt) 的模塊(Module)來(lái)實(shí)現(xiàn)的, 它允許 MCU 以全雙工的同步串行方式, 與各種外圍設(shè)備進(jìn)行高速數(shù)據(jù)通信。</p><p>　　圖6 SPI寫(xiě)時(shí)序圖</p><p>　?。ū緢D片摘自ATK-0.96’ OLED模塊用戶手冊(cè)_V1.0）</p><p><b>　　CS：片選信號(hào)</b></p><p>　　RS

48、T(RES)：硬復(fù)位</p><p>　　DC：命令/數(shù)據(jù)標(biāo)志（0，讀寫(xiě)命令；1，讀寫(xiě)數(shù)據(jù)）。 </p><p>　　SCLK(D0)：串行時(shí)鐘線SCLK</p><p>　　SDIN(D1)：串行數(shù)據(jù)線SDIN </p><p>　　圖7 MCU與OLED通信原理圖</p><p>　　在發(fā)送數(shù)據(jù)時(shí), MCU要先向內(nèi)

49、部緩沖區(qū)里寫(xiě)入將要發(fā)送出去的數(shù)據(jù), 這些數(shù)據(jù)會(huì)被移位寄存器自動(dòng)移入內(nèi)部緩沖區(qū)里的數(shù)據(jù), 然后這些數(shù)據(jù)通過(guò)MCU的SDO管腳傳給OLED的SDI 管腳, OLED的移位寄存器則把從SDI管腳接收到的數(shù)據(jù)移入其內(nèi)部緩沖區(qū) 里. 與此同時(shí), OLED的移位寄存器里面的數(shù)據(jù)根據(jù)每次接收數(shù)據(jù)的大小, 通過(guò)OLED的SDO 發(fā)往 MCU的SDI, MCU的移位寄存器再把從 SDI 接收的數(shù)據(jù)移入內(nèi)部緩沖區(qū).在單次數(shù)據(jù)傳輸完成之后,內(nèi)部緩沖區(qū)會(huì)將數(shù)

50、據(jù)移出到相應(yīng)的讀寄存器中，程序可以通過(guò)從MCU讀寄存器讀取與OLED數(shù)據(jù)交換得到的數(shù)據(jù)。在這個(gè)過(guò)程中時(shí)鐘信號(hào)又MCU的定時(shí)器生成通過(guò)SCK線傳輸給OLED模塊，從而實(shí)現(xiàn)同步。</p><p>　　MCU與OLED的數(shù)據(jù)交換之后向其控制寄存器中寫(xiě)入相應(yīng)參數(shù)進(jìn)行初始化，之后就可以對(duì)顯示模塊的界面進(jìn)行相應(yīng)的配置。</p><p><b>　　界面程序分析：</b></

51、p><p>　　typedef struct </p><p><b>　　{</b></p><p><b>　　u8 SET;</b></p><p><b>　　u8 mode;</b></p><p><b>　　u8 point;<

52、;/b></p><p>　　u8 pointMAX;</p><p>　　u8 Numallow;</p><p>　　u8 NumFlag;</p><p>　　u8 pointFlag;</p><p>　　float Value;</p><p><b>　　}<

53、/b></p><p><b>　　KeyNum;</b></p><p>　　extern KeyNum KEY_FLAG;</p><p>　　以上為按鍵的結(jié)構(gòu)體定義以及聲明。其中定義了按鍵配合顯示的各種標(biāo)志位，以及鍵盤輸入值的緩存空間Value。</p><p>　　void window_main(void

54、)</p><p><b>　　{</b></p><p>　　switch(KEY_FLAG.mode)</p><p><b>　　{ </b></p><p>　　case 0:window0();break;</p><p>　　case 1:window1();

55、break;</p><p>　　case 2:window2();break;</p><p>　　case 3:window3();break;</p><p>　　case 4:window4();break;</p><p>　　case 5:window5() ;break;</p><p>　　defaul

56、t:break;</p><p><b>　　}</b></p><p><b>　　}</b></p><p>　　這是程序主循環(huán)中的內(nèi)容，用于根據(jù)按鍵輸入的值確定系統(tǒng)將要執(zhí)行的動(dòng)作以及進(jìn)入對(duì)應(yīng)的功能設(shè)置界面，其中window0()為主界面，window1()~window5()為相應(yīng)的功能界面。</p>

57、<p>　　void window0(void)</p><p><b>　　{</b></p><p>　　if(KEY_FLAG.mode==0)</p><p><b>　　{ </b></p><p>　　if(!KEY_FLAG.pointFlag)</p>&

58、lt;p><b>　　{ </b></p><p>　　OLED_Fill(0x00); //清屏</p><p>　　OLED_P6x8Str(0,0, (uint8_t *)"1.request1 and 2");</p><p>　　OLED_P6x8Str(0,1,(uint8_t *)"2.req

59、uest3 ");</p><p>　　OLED_P6x8Str(0,2,(uint8_t *)"3.request4 ");</p><p>　　OLED_P6x8Str(0,3,(uint8_t *)"4. request5 ");</p><p>　　OLED_P6x8Str(0,4,(uint8_t *

60、)"5. request6");</p><p>　　OLED_P6x8Str(0,7,(uint8_t *)"mode= 000 "); </p><p>　　KEY_FLAG.pointFlag=1; </p><p><b>　　}</b></p><p>　　OLE

61、D_P6x8Num_8bit(36,7,mode);</p><p><b>　　}</b></p><p><b>　　}</b></p><p>　　該函數(shù)設(shè)置系統(tǒng)主界面里面顯示的元素以及實(shí)時(shí)的對(duì)mode這個(gè)功能字進(jìn)行顯示。</p><p>　　void window1(void)</p&

62、gt;<p><b>　　{</b></p><p>　　if(KEY_FLAG.mode!=0)</p><p><b>　　{ </b></p><p>　　if(!KEY_FLAG.pointFlag)</p><p><b>　　{ </b></

63、p><p>　　OLED_Fill(0x00); </p><p>　　OLED_P6x8Str(0,0,(uint8_t *)"1.request1 and 2");</p><p>　　OLED_P6x8Str(0,2,(uint8_t *)"Length: ");</p><p>　　OLE

64、D_P6x8Str(0,4,(uint8_t *)"--------------");</p><p>　　KEY_FLAG.pointFlag=1;</p><p>　　KEY_FLAG.point=1;</p><p>　　KEY_FLAG.pointMAX=3;</p><p>　　KEY_FLAG.Numallow

65、=1;</p><p><b>　　}</b></p><p>　　switch(KEY_FLAG.point)</p><p><b>　　{</b></p><p>　　case 0: ;break;</p><p>　　case 1:{OLED_P6x8Num_8bit

66、(42,2, KEY_FLAG.Value);}break;</p><p>　　case 2:{Clength=KEY_FLAG.Value;</p><p>　　OLED_P6x8Str(110,2,(uint8_t*)"OK");</p><p>　　KEY_FLAG.Value=0;</p><p>　　KEY_F

67、LAG.point++;</p><p><b>　　}break;</b></p><p>　　case 3:{ </p><p>　　KEY_FLAG.Numallow=0;</p><p>　　if(KEY_FLAG.SET) </p><p><b>　　{ </b&g

68、t;</p><p>　　OLED_P6x8Str(0,7,(uint8_t *)" SET!!!!");</p><p>　　request1();</p><p><b>　　}</b></p><p><b>　　}break;</b></p>&l

69、t;p>　　default:break;</p><p><b>　　}</b></p><p>　　功能界面配置函數(shù)，這個(gè)函數(shù)實(shí)現(xiàn)的功能是對(duì)擺長(zhǎng)的設(shè)定，并且一鍵啟動(dòng)。其中首先是對(duì)圖像界面進(jìn)行了設(shè)置，然后進(jìn)行鍵盤輸入值的實(shí)時(shí)顯示，以及鍵值和相應(yīng)的動(dòng)作設(shè)備直接的數(shù)據(jù)傳遞，實(shí)現(xiàn)通過(guò)鍵盤對(duì)單擺擺長(zhǎng)，停擺等動(dòng)作的控制。這個(gè)函數(shù)是一個(gè)功能窗口模版，其他的功能配置一樣是通過(guò)

70、這個(gè)函數(shù)實(shí)現(xiàn)，使用時(shí)只需修改if(KEY_FLAG.SET)｛｝這個(gè)大括號(hào)的內(nèi)容以及界面的顯示字符。</p><p>　　4.4 MPU6050的初始化及相關(guān)程序段分析</p><p>　　MPU6050整合了三軸陀螺儀、三軸加速度計(jì)，可同時(shí)對(duì)角速度和加速度進(jìn)行測(cè)量，初始化MPU6050首先就需要在MCU上實(shí)現(xiàn)數(shù)據(jù)傳輸?shù)耐ㄐ艆f(xié)議。</p><p>　　通信協(xié)議為I

71、IC總線協(xié)議，該協(xié)議的硬件連接十分簡(jiǎn)單，只要求兩條總線線路 一條串行數(shù)據(jù)線 SDA 一條串行時(shí)鐘線 SCL。IIC是一個(gè)真正的多主機(jī)總線 如果兩個(gè)或更多主機(jī)同時(shí)初始化數(shù)據(jù)傳輸可以通過(guò)沖突檢測(cè)和仲裁防止數(shù)據(jù)被破壞。</p><p>　　在 I2C 總線中 有起始條件和停止條件，起始條件是指SCL線是高電平時(shí) SDA 線從高電平向低電平切換。停止條件是指當(dāng) SCL 是高電平時(shí) SDA 線由低電平向高電平切換。起始和停

72、止條件都由MCU產(chǎn)生。</p><p><b>　　起始條件：</b></p><p><b>　　SDA_H;</b></p><p><b>　　SCL_H;</b></p><p>　　_NOP;//適當(dāng)延時(shí)</p><p><b>　　

73、SDA_L;</b></p><p><b>　　結(jié)束條件：</b></p><p><b>　　SDA_L;</b></p><p><b>　　SCL_H;</b></p><p>　　_NOP; //適當(dāng)延時(shí)</p><p><b

74、>　　SDA_H;</b></p><p>　　SDA 線電平要在SCL 線的高電平周期保持不變。數(shù)據(jù)線的高或低電平狀態(tài)只有在 SCL 線的時(shí)鐘信號(hào)是低電平時(shí)才能改變。</p><p>　　MCU在GPIO上生成開(kāi)始條件后，SCL發(fā)生一次下降沿時(shí)，MCU從SDA讀入一位數(shù)據(jù)，連續(xù)8次完成1byte的一個(gè)字節(jié)數(shù)據(jù)傳輸。每次次發(fā)送數(shù)據(jù)必須為 8 位一個(gè)字節(jié)。每次傳輸可以發(fā)送的

75、字節(jié)數(shù)量不受限制 每個(gè)字節(jié)后必須跟一個(gè)響應(yīng)位。數(shù)據(jù)傳輸順序是從高位到低位。如果在數(shù)據(jù)傳輸過(guò)程中，MCU不能立即接受數(shù)據(jù)。例如有一個(gè)一個(gè)內(nèi)部中斷服務(wù)程序優(yōu)先級(jí)在數(shù)據(jù)傳輸程序之上，可以使時(shí)鐘線 SCL 保持低電平讓主機(jī)進(jìn)入等待狀態(tài) 當(dāng)從機(jī)準(zhǔn)備好接收下一個(gè)數(shù)據(jù)字節(jié)并釋放時(shí)鐘線 SCL 后，數(shù)據(jù)繼續(xù)傳輸。</p><p>　　IIC協(xié)議配置完畢之后就開(kāi)始對(duì)MPU6050的寄存器進(jìn)行配置：</p><p

76、>　　u8 InitMPU6050(void)</p><p><b>　　{</b></p><p><b>　　u8 date;</b></p><p>　　date = Single_Write(MPU6050_ADDRESS, PWR_MGMT_1, 0x00); </p><p>

77、<b>　　//解除休眠狀態(tài)</b></p><p>　　date += Single_Write(MPU6050_ADDRESS, SMPLRT_DIV, 0x07); </p><p>　　date += Single_Write(MPU6050_ADDRESS, cONFIG, 0x03); </p><p>&l

78、t;b>　　//低通濾波</b></p><p>　　date += Single_Write(MPU6050_ADDRESS, GYRO_CONFIG, 0x10); </p><p>　　//陀螺儀量程選擇 +-1000</p><p>　　date += Single_Write(MPU6050_ADDRESS, ACCEL_CONFI

79、G, 0x09); </p><p>　　//加速度計(jì)量程選擇 +-4G</p><p>　　return date;</p><p><b>　　}</b></p><p>　　在寄存器配置完成之后，MPU6050就已經(jīng)開(kāi)始工作，通過(guò)對(duì)其數(shù)據(jù)緩存器進(jìn)行讀操作就能得到相應(yīng)的角速度和加速度。讀取完畢后的數(shù)據(jù)不能直接使用，

80、其中的噪聲很大，需要用濾波算法將其有效信號(hào)提取出來(lái)再進(jìn)行使用。</p><p>　　double IIR_I_Filter(double InData, double *x, double *y, double *b, short nb, double *a, short na)</p><p><b>　　{</b></p><p>　　do

81、uble z1,z2;</p><p><b>　　short i;</b></p><p>　　double OutData;</p><p>　　for(i=nb-1; i>0; i--)</p><p><b>　　{</b></p><p>　　x[i]=x[i

82、-1];</p><p><b>　　}</b></p><p>　　x[0] = InData;</p><p>　　for(z1=0,i=0; i<nb; i++)</p><p><b>　　{</b></p><p>　　z1 += x[i]*b[i];<

83、/p><p><b>　　}</b></p><p>　　for(i=na-1; i>0; i--)</p><p><b>　　{</b></p><p>　　y[i]=y[i-1];</p><p><b>　　}</b></p>&

84、lt;p>　　for(z2=0,i=1; i<na; i++)</p><p><b>　　{</b></p><p>　　z2 += y[i]*a[i];</p><p><b>　　}</b></p><p>　　y[0] = z1 - z2; </p><p&g

85、t;　　OutData = y[0];</p><p>　　return OutData;</p><p><b>　　}</b></p><p>　　該函數(shù)實(shí)現(xiàn)的是一個(gè)IIR直接I型濾波器，InData為當(dāng)前數(shù)據(jù)，*x為未濾波數(shù)據(jù)緩存區(qū)，*y為已濾波數(shù)據(jù)緩存區(qū)，*b、*a為參數(shù)緩存區(qū)，nb、na 為濾波器階，OutData為輸出結(jié)果。

86、 </p><p>　　第五章 測(cè)試方案與測(cè)試結(jié)果</p><p>　　5.1 測(cè)試所用儀器</p><p><b>　　5.2 測(cè)試方案</b></p><p>　?。?） 從靜止開(kāi)始，進(jìn)行幅度可控的擺動(dòng)，畫(huà)出長(zhǎng)度可設(shè)置，偏差不大于±2.5cm 的直線段，并且具有較好的重復(fù)性，用上位機(jī)觀察角速度波形是否發(fā)散

87、（或收斂）記錄風(fēng)力擺達(dá)到要求所需時(shí)間，實(shí)際直線段長(zhǎng)度。</p><p>　?。?）風(fēng)力擺從靜止開(kāi)始，設(shè)定擺動(dòng)方向，按照設(shè)置的方向（角度）擺動(dòng)，畫(huà)出不短于20cm 的直線段。記錄風(fēng)力擺達(dá)到要求所需時(shí)間，實(shí)際直線段長(zhǎng)度。</p><p>　?。?） 將風(fēng)力擺拉起一定角度（30°~45°）放開(kāi)，記錄風(fēng)力擺制動(dòng)達(dá)到靜止?fàn)顟B(tài)的時(shí)間。</p><p>　　（

88、4）以風(fēng)力擺靜止時(shí)激光筆的光點(diǎn)為圓心，驅(qū)動(dòng)風(fēng)力擺用激光筆在地面畫(huà)</p><p>　　圓，重復(fù)3次；在 15~35cm 范圍內(nèi)設(shè)置圓半徑，記錄風(fēng)力擺達(dá)到要求所需時(shí)間，</p><p>　　設(shè)定圓半徑，實(shí)際園半徑。</p><p>　?。?）作圓周運(yùn)動(dòng)，在距離風(fēng)力擺 1~2m 距離內(nèi)用一臺(tái) 50~60W 臺(tái)扇在水平方向吹向風(fēng)力擺，臺(tái)扇吹 5s 后停止，記錄風(fēng)力擺恢復(fù)發(fā)

89、揮設(shè)定規(guī)定的圓周運(yùn)動(dòng)所需時(shí)間。設(shè)定圓半徑，實(shí)際園半徑。</p><p><b>　　5.3 誤差分析</b></p><p>　　機(jī)械結(jié)構(gòu)不均勻使風(fēng)力擺靜止時(shí)重心不位于中軸線上，擺角解算出的角度與實(shí)際角度有誤差。</p><p><b>　　致謝</b></p><p>　　這篇論文是三個(gè)月來(lái)在老師

90、悉心指導(dǎo)下綜合運(yùn)用大學(xué)這幾年來(lái)所學(xué)的各種專業(yè)知識(shí)與實(shí)踐相結(jié)合的結(jié)果。</p><p>　　這篇論文能夠完成，首先要感謝唐XX老師的悉心指導(dǎo)。唐老師見(jiàn)識(shí)廣博，為我提供了大量的資源以及更為廣闊的發(fā)展空間。并開(kāi)放實(shí)驗(yàn)室，讓我能將純粹理論與實(shí)際聯(lián)合在一起，得出最正確的結(jié)論。</p><p>　　從剛開(kāi)始的什么都不懂到現(xiàn)在論文就要完成，其中的過(guò)程有許多磕磕碰碰。經(jīng)歷過(guò)后真正的感覺(jué)以往學(xué)到的理論知識(shí)，

91、不僅僅只是理論。似乎許多事物都能透過(guò)現(xiàn)象看到它的工作本質(zhì)。在這整個(gè)過(guò)程中還有許多可敬的朋友、同學(xué)、師長(zhǎng)給了幫助，在這里請(qǐng)接受我最誠(chéng)摯的謝意，由于筆者能力有限，文章中可能有許多錯(cuò)誤或疏漏之處，懇請(qǐng)各位老師予以斧正。</p><p><b>　　參考文獻(xiàn)</b></p><p>　　【1】 胡壽松.自動(dòng)控制原理（第六版）.科學(xué)出版社</p><p>

92、;　　【2】 秦永元.慣性導(dǎo)航（第二版）.科學(xué)出版社</p><p>　　【3】 宋 巖.ARM Cortex-M3權(quán)威指南.北京航空航天大學(xué)出版社</p><p>　　【4】 劉火良.STM32庫(kù)開(kāi)發(fā)實(shí)戰(zhàn)指南.機(jī)械工業(yè)出版社</p><p>　　【5】 童詩(shī)白.模擬電子技術(shù)基礎(chǔ)(第四版）.高等教育出版社 </p><p>　　【6】 閻石

93、.數(shù)字電子技術(shù)基礎(chǔ).清華大學(xué)電子學(xué)教研組編</p><p>　　【7】 張華清等.信號(hào)與系統(tǒng)分析.機(jī)械工業(yè)出版社</p><p>　　【8】 邱關(guān)源等.電路.高等教育出版社 </p><p>　　【9】 李元春.計(jì)算機(jī)控制系統(tǒng) .高等教育出版社</p><p>　　【10】譚浩強(qiáng).C程序設(shè)計(jì).北京：清華大學(xué)出版社</p><

94、;p>　　【11】 楊建華 楊艷琴.MSP430系列16位超低功耗單片機(jī)原理與實(shí)踐.北京航空航天大學(xué)出版社</p><p>　　【12】羅杰 謝自美.電子線路 設(shè)計(jì)·實(shí)驗(yàn)·測(cè)試.電子工業(yè)出版社</p><p>　　【13】黃根春等.全國(guó)大學(xué)生電子設(shè)計(jì)競(jìng)賽教程：基于TI器件設(shè)計(jì)方法.電子工業(yè)出版社</p><p>　　【14】胡壽松.自動(dòng)控制

95、原理（第五版）.科學(xué)出版社</p><p>　　【15】蘇金明 . MATLAB7.0實(shí)用指南[M ].電子工業(yè)出版社</p><p>　　【16】范超毅等.步進(jìn)電機(jī)的選型與計(jì)算.機(jī)床與液壓</p><p>　　【17】金以蕊.過(guò)程控制[M].清華大學(xué)出版社</p><p>　　【18】徐科軍.傳感器與檢測(cè)技術(shù)（第二版）.北京電子工業(yè)出版社&

96、lt;/p><p><b>　　附錄</b></p><p>　　STM32VCT6原理圖</p><p>　　//********************************main.c********************************//</p><p>　　#include "include

97、.h" </p><p>　　#define MAX(v1,v2) v1>v2?v1:v2</p><p>　　extern u8 sentDateFlag;</p><p>　　u8 Status_Senser_RC_cnt=0,data_cnt=0;</p><p>　　u16 youa=0;</p>&l

98、t;p>　　void request1(void)</p><p><b>　　{</b></p><p>　　Swing(0,600);</p><p>　　TIM2->CCR1 = 0;</p><p>　　TIM2->CCR1 = 0;</p><p>　　TIM2->

99、;CCR1 = 0;</p><p>　　TIM2->CCR1 = 0;</p><p>　　EnTIM3(); </p><p><b>　　while(1)</b></p><p><b>　　{</b></p><p>　　if(sensor.gyro.ori

100、gin.x<100)</p><p>　　TIM2->CCR1=290;</p><p><b>　　else</b></p><p>　　TIM2->CCR1=0;</p><p>　　if(sentDateFlag) </p><p><b>　　{</b&g

101、t;</p><p>　　sentDateFlag = 0;</p><p>　　Status_Senser_RC_cnt++;</p><p>　　data_cnt++;</p><p>　　if(Status_Senser_RC_cnt==4)</p><p><b>　　{</b></

102、p><p>　　Status_Senser_RC_cnt = 0;</p><p>　　Send_Status = 1;</p><p>　　Send_Senser = 1;</p><p>　　Send_RCData=1;</p><p><b>　　}</b></p><p&g

103、t;　　if(data_cnt==3)</p><p><b>　　{</b></p><p>　　data_cnt=0;</p><p>　　Data_Exchange();</p><p><b>　　}</b></p><p><b>　　}</b&

104、gt;</p><p>　　BATTDispaly();</p><p><b>　　}</b></p><p><b>　　}</b></p><p>　　void request2(u16 l)</p><p><b>　　{</b></p&g

105、t;<p>　　float max_h=0,max_s=0;</p><p>　　u8 cnt=30;</p><p>　　u16 pid_out;</p><p>　　float e_h=0,sum_e=0;</p><p>　　pid_out=l*7/13;</p><p>　　Swing(0,l);

106、</p><p>　　TIM2->CCR1 = 0;</p><p>　　TIM2->CCR2 = 0;</p><p>　　TIM2->CCR3 = 0;</p><p>　　TIM2->CCR4 = 0;</p><p>　　EnTIM3(); </p><p

107、>　　max_h=atan((float)l/2/880)*57.3;</p><p><b>　　while(1)</b></p><p><b>　　{</b></p><p><b>　　if(cnt)</b></p><p><b>　　{</b&

108、gt;</p><p>　　if(max_s<angle.roll)</p><p>　　max_s = angle.roll;</p><p>　　if(angle.roll<1&&angle.roll>(-1))</p><p><b>　　cnt--;</b></p>

109、<p><b>　　}</b></p><p>　　if(sensor.gyro.origin.x<(100))</p><p>　　TIM2->CCR1=pid_out;</p><p><b>　　else</b></p><p>　　TIM2->CCR1=0;&l

110、t;/p><p>　　if(sentDateFlag) </p><p><b>　　{</b></p><p>　　sentDateFlag = 0;</p><p>　　Status_Senser_RC_cnt++;</p><p>　　data_cnt++;</p><p&

111、gt;　　if(Status_Senser_RC_cnt==4)</p><p><b>　　{</b></p><p>　　Status_Senser_RC_cnt = 0;</p><p>　　Send_Status = 1;</p><p>　　Send_Senser = 1;</p><p&g

112、t;　　Send_RCData=1;</p><p><b>　　}</b></p><p>　　if(data_cnt==3)</p><p><b>　　{</b></p><p>　　data_cnt=0;</p><p>　　Data_Exchange();<

113、;/p><p><b>　　}</b></p><p><b>　　}</b></p><p>　　BATTDispaly(); </p><p>　　if(cnt==0) </p><p><b>　　{</b></p><p>　

114、　e_h=max_h-max_s;</p><p>　　sum_e+=e_h;</p><p>　　if(sum_e>150) sum_e=150;</p><p>　　pid_out = (28-l/100)*e_h+0.2*sum_e;</p><p>　　if(pid_out<0) pid_out=0;</p>

115、<p><b>　　cnt=30;</b></p><p><b>　　max_s=0;</b></p><p><b>　　}</b></p><p>　　if(sensor.gyro.origin.x<(100))</p><p>　　TIM2->C

116、CR1=pid_out;</p><p><b>　　else</b></p><p>　　TIM2->CCR1=0;</p><p><b>　　}</b></p><p><b>　　}</b></p><p>　　u8 flag_stop=1

117、,flag_stop2=1;</p><p>　　u16 time_requet4;</p><p>　　void request4(u16 time,u8 mode)</p><p><b>　　{</b></p><p>　　//float kp=3.5,kp_w=10;</p><p>　

118、　float kp=2.7,kp_w=10;</p><p>　　//u16 time=400,pid_out;</p><p>　　u16 pid_out;</p><p>　　time_requet4=time;</p><p>　　request4_flag=1;</p><p><b>　　EnTI

119、M3();</b></p><p>　　GPIO_SetBits(GPIOB,GPIO_Pin_0);</p><p>　　delay(1000);</p><p>　　GPIO_ResetBits(GPIOB,GPIO_Pin_0);</p><p><b>　　while(1)</b></p>

120、<p><b>　　{</b></p><p>　　if(flag_stop2)</p><p><b>　　{</b></p><p>　　if(sensor.gyro.origin.x<0)</p><p><b>　　{</b></p>

121、<p>　　pid_out=-1*kp* sensor.gyro.origin.x-kp_w*angle.roll;</p><p>　　if(pid_out>2499)pid_out=2499;</p><p>　　TIM2->CCR3 =pid_out ;</p><p>　　TIM2->CCR1 = 0 ;</p>&

122、lt;p>　　//delay(time);</p><p>　　//TIM2->CCR3 = 0;</p><p><b>　　}</b></p><p>　　if(sensor.gyro.origin.x>0)</p><p><b>　　{</b></p&

123、gt;<p>　　pid_out=kp* sensor.gyro.origin.x+kp_w*angle.roll;</p><p>　　if(pid_out>2499)pid_out=2499;</p><p>　　TIM2->CCR1 = pid_out;</p><p>　　TIM2->CCR3 = 0 ;</p>

124、<p>　　//delay(time);</p><p>　　//TIM2->CCR1 = 0;</p><p><b>　　}</b></p><p>　　if(sensor.gyro.origin.y<0)</p><p><b>　　{</b></

125、p><p>　　pid_out=-1*kp* sensor.gyro.origin.y-(kp_w-0.1)*angle.pitch;</p><p>　　if(pid_out>2499)pid_out=2499;</p><p>　　TIM2->CCR4 = pid_out;</p><p>　　TIM2->CCR2 = 0

126、;</p><p>　　//delay(time);</p><p>　　//TIM2->CCR4 = 0;</p><p><b>　　}</b></p><p>　　if(sensor.gyro.origin.y>0)</p><p><b>　　{<