多旋翼自主飛行器報(bào)告(共20頁(yè))

1、PAGE PAGE 242015年全國(guó)(qun u)大學(xué)生電子設(shè)計(jì)競(jìng)賽多旋翼自主(zzh)飛行器（C題）2015年8月15日摘要(zhiyo) 四旋翼飛行器是一種結(jié)構(gòu)新穎、性能優(yōu)越的垂直起降飛行器，具有操作 靈活、帶負(fù)載(fzi)能力強(qiáng)等特點(diǎn)，具有重要的軍事和民用價(jià)值，本系統(tǒng)(xtng)以瑞薩R5F100LE作為四旋翼自主飛行器控制的核心。由電源模塊、電機(jī)調(diào)速控制模塊、電機(jī)模塊、MPU9150模塊、飛行器控制模塊等構(gòu)成。飛行控制模塊包括角度傳感器、陀螺儀，瑞薩MCU綜合飛行器模塊和傳感器檢測(cè)模塊的信息，通過(guò)控制4個(gè)直流無(wú)刷電機(jī)轉(zhuǎn)速來(lái)實(shí)現(xiàn)飛行器的欠驅(qū)動(dòng)系統(tǒng)飛行。在動(dòng)力學(xué)模型的基礎(chǔ)上，將小型四旋翼

2、飛行器實(shí)時(shí)控制算法分為兩個(gè)PID控制回路，即位置控制回路和姿態(tài)控制回路。測(cè)試結(jié)果表明系統(tǒng)可通過(guò)各個(gè)模塊的配合實(shí)現(xiàn)對(duì)電機(jī)的精確控制，具有平均速度快、定位誤差小、運(yùn)行較為穩(wěn)定等特點(diǎn)。 關(guān)鍵詞： 四旋翼飛行器；MPU9150；瑞薩R5F100LE；PID目 錄 TOC o 1-3 h z u HYPERLINK l _Toc427414960 一、系統(tǒng)(xtng)方案 PAGEREF _Toc427414960 h 4 HYPERLINK l _Toc427414961 1、系統(tǒng)方案論證與控制(kngzh)方案的選擇 PAGEREF _Toc427414961 h 4 HYPERLINK l _To

3、c427414962 1.1 飛行器控制系統(tǒng)(kn zh x tn)模塊 PAGEREF _Toc427414962 h 4 HYPERLINK l _Toc427414963 1.2 電源模塊的選擇與論證 PAGEREF _Toc427414963 h 4 HYPERLINK l _Toc427414964 1.3電機(jī)模塊的選擇與論證 PAGEREF _Toc427414964 h 5 HYPERLINK l _Toc427414965 1.4電機(jī)調(diào)速控制模塊 PAGEREF _Toc427414965 h 5 HYPERLINK l _Toc427414966 二、系統(tǒng)理論分析與計(jì)算 PA

4、GEREF _Toc427414966 h 6 HYPERLINK l _Toc427414967 四旋翼飛行器動(dòng)力學(xué)模型 PAGEREF _Toc427414967 h 6 HYPERLINK l _Toc427414968 2.2 PID控制算法結(jié)構(gòu)分析 PAGEREF _Toc427414968 h 7 HYPERLINK l _Toc427414969 三、電路與程序設(shè)計(jì) PAGEREF _Toc427414969 h 9 HYPERLINK l _Toc427414970 1、電路的設(shè)計(jì) PAGEREF _Toc427414970 h 9 HYPERLINK l _Toc427414

5、971 （1）系統(tǒng)總體框圖 PAGEREF _Toc427414971 h 9 HYPERLINK l _Toc427414972 （2）飛行控制系統(tǒng)實(shí)物圖與電路原理圖 PAGEREF _Toc427414972 h 9 HYPERLINK l _Toc427414973 （3）給R5F100LE單片機(jī)為核心提供的5V電源系統(tǒng)框圖與電路原理圖 PAGEREF _Toc427414973 h 11 HYPERLINK l _Toc427414974 （4）總電源 PAGEREF _Toc427414974 h 12 HYPERLINK l _Toc427414975 （5）MPU9150模塊 P

6、AGEREF _Toc427414975 h 12 HYPERLINK l _Toc427414976 2、程序的設(shè)計(jì) PAGEREF _Toc427414976 h 13 HYPERLINK l _Toc427414977 四、測(cè)試方案與結(jié)果分析 PAGEREF _Toc427414977 h 14 HYPERLINK l _Toc427414978 4.1 測(cè)試設(shè)備方案 PAGEREF _Toc427414978 h 14 HYPERLINK l _Toc427414979 4.2 測(cè)試結(jié)果 PAGEREF _Toc427414979 h 15 HYPERLINK l _Toc427414

7、980 五、 總結(jié) PAGEREF _Toc427414980 h 16 HYPERLINK l _Toc427414981 六、參考文獻(xiàn) PAGEREF _Toc427414981 h 16 HYPERLINK l _Toc427414982 七、附錄 PAGEREF _Toc427414982 h 16多旋翼自主(zzh)飛行器（C題）【高職組】一、系統(tǒng)(xtng)方案本系統(tǒng)(xtng)主要由飛行器控制模塊、電源模塊、電機(jī)模塊、電機(jī)調(diào)速控制模塊、MPU9150模塊等構(gòu)成5大模塊組織，下面分別論證這幾個(gè)模塊的選擇。1、系統(tǒng)方案論證與控制方案的選擇1.1 飛行器控制系統(tǒng)模塊 中心控制模塊即飛行

8、控制系統(tǒng)的核心處理器作為整個(gè)系統(tǒng)的核心控制部分，主要負(fù)責(zé)采集傳感器檢測(cè)到的姿態(tài)角速率(俯仰角速率、橫滾角速率和偏航角速率)、三軸的線加速度和航向信息并實(shí)時(shí)解算；根據(jù)檢測(cè)到的飛行信息，結(jié)合既定的控制方案，計(jì)算輸出控制量；通過(guò)無(wú)線通信模塊與地面站進(jìn)行數(shù)據(jù)的傳輸，實(shí)現(xiàn)接收控制命令改變飛行狀態(tài)和下傳飛行狀態(tài)數(shù)據(jù)。 方案一：主控板使用pcduino。其內(nèi)存大而且板子體積較小，重量較輕，對(duì)四旋翼的載重量要求較低。但是它對(duì)電源的要求較高，而且I/O口較少，我們剛開(kāi)始學(xué)習(xí)使用，不是很熟悉，對(duì)于四旋翼的需要不夠。方案二：主控板使用瑞薩MCU綜合飛行器模塊和傳感器檢測(cè)模塊的信息, 使用R5F100LE作為四旋翼自

9、主飛行器控制的核心通過(guò)控制4個(gè)直流無(wú)刷電機(jī)轉(zhuǎn)速來(lái)實(shí)現(xiàn)飛行器的欠驅(qū)動(dòng)系統(tǒng)飛行。在動(dòng)力學(xué)模型的基礎(chǔ)上，將小型四旋翼飛行器實(shí)時(shí)控制算法分為兩個(gè)PID控制回路，即位置控制回路和姿態(tài)控制回路。測(cè)試結(jié)果表明系統(tǒng)可通過(guò)各個(gè)模塊的配合實(shí)現(xiàn)對(duì)電機(jī)的精確控制，具有平均速度快、定位誤差小、運(yùn)行較為穩(wěn)定等特點(diǎn)。比較以上二種方案，方案二占有很大優(yōu)勢(shì)，及本組成員的水平和競(jìng)賽所提供的元器件所以選擇方案二。1.2 電源模塊的選擇與論證 電源模塊在系統(tǒng)中的地位是極其重要的，四旋翼無(wú)人直升機(jī)要穩(wěn)定工作必須有穩(wěn)定的電源供給作為保障，為系統(tǒng)的各個(gè)模塊提供動(dòng)力。穩(wěn)定的電源可以使系統(tǒng)在各種環(huán)境下長(zhǎng)時(shí)間穩(wěn)定的工作，而如果電源模塊設(shè)計(jì)得不夠

10、合理，那么就像在系統(tǒng)中埋下了一顆不定時(shí)炸彈，系統(tǒng)隨時(shí)都可能因此而崩潰。所以電源模塊的設(shè)計(jì)必須非常慎重，以保證系統(tǒng)工作的穩(wěn)定性。 在本控制系統(tǒng)中，根據(jù)各模塊工作的要求有如下方案:方案一：采用鉛酸電池：鉛酸電池（VRLA），是一種電極主要由鉛及其氧化物制成，電解液是硫酸溶液的蓄電池。鉛酸電池荷電狀態(tài)下，正極主要成分為二氧化鉛，負(fù)極(fj)主要成分為鉛；放電狀態(tài)下，正負(fù)極的主要成分均為硫酸鉛。 方案(fng n)二：采用鋰聚合物電池：它也是鋰離子電池的一種，但是(dnsh)與液鋰電池(Li-ion)相比具有能量密度高、更小型化、超薄化、輕量化，以及高安全性和低成本等多種明顯優(yōu)勢(shì)，是一種新型電池。在形

11、狀上，鋰聚合物電池具有超薄化特征，可以配合各種產(chǎn)品的需要，制作成任何形狀與容量的電池。該類(lèi)電池可以達(dá)到的最小厚度可達(dá)0.5mm。相同容量的鉛酸電池比鋰聚合物電池便宜很多，但重量比鋰聚合物電池重很多！而飛行器的載重能力有限，所以最終選擇方案二鋰聚合物電池。1.3電機(jī)模塊的選擇與論證四旋翼無(wú)人飛行器是通過(guò)控制四個(gè)不同無(wú)刷直流電機(jī)的轉(zhuǎn)速，達(dá)到控制四旋翼無(wú)人飛行器的飛行姿態(tài)和位置，與傳統(tǒng)直升機(jī)通過(guò)控制舵機(jī)來(lái)改變螺旋槳的槳距角，達(dá)到控制直升機(jī)的目的不同。在電機(jī)的選型上，主要有直流有刷電機(jī)和直流無(wú)刷電機(jī)兩種。方案一：直流有刷電機(jī)是當(dāng)前普遍使用的一種直流電機(jī)，它的驅(qū)動(dòng)電路簡(jiǎn)單、控制方法成熟，但是直流有刷電機(jī)

12、使用電刷進(jìn)行換向，換向時(shí)電刷與線圈觸電存在機(jī)械接觸，電機(jī)長(zhǎng)時(shí)間高速轉(zhuǎn)動(dòng)使極易因磨損導(dǎo)致電氣接觸不良等問(wèn)題，而且有刷電機(jī)效率低、力矩小、重量大，不適合對(duì)功率重量比敏感的電動(dòng)小型飛行器。方案二：直流無(wú)刷電機(jī)能量密度高、力矩大、重量輕，采用非接觸式的電子換向方法，消除了電刷磨損，較好地解決了直流有刷電機(jī)的缺點(diǎn)，適用于對(duì)功率重量比敏感的用途，同時(shí)增強(qiáng)了電機(jī)的可靠性。所以選擇直流無(wú)刷電機(jī)作為動(dòng)力源。1.4電機(jī)調(diào)速控制(kngzh)模塊方案(fng n)一:采用電阻網(wǎng)絡(luò)或數(shù)字電位器調(diào)整電動(dòng)機(jī)的分壓，從而達(dá)到調(diào)速的目的。但是電阻網(wǎng)絡(luò)只能實(shí)現(xiàn)有級(jí)調(diào)速，而數(shù)字電阻的元器件價(jià)格比較昂貴。更主要的問(wèn)題在于一般電動(dòng)機(jī)

13、的電阻很小，但電流很大；分壓不僅(bjn)會(huì)降低效率，而且實(shí)現(xiàn)很困難。 方案二:采用繼電器對(duì)電動(dòng)機(jī)的開(kāi)或關(guān)進(jìn)行控制，通過(guò)開(kāi)關(guān)的切換對(duì)小車(chē)的速度進(jìn)行調(diào)整。這個(gè)方案的優(yōu)點(diǎn)是電路較為簡(jiǎn)單，缺點(diǎn)是繼電器的響應(yīng)時(shí)間慢、機(jī)械結(jié)構(gòu)易損壞、壽命較 短、可靠性不高。方案三:采用全橋驅(qū)動(dòng)PWM電路。這種驅(qū)動(dòng)的優(yōu)點(diǎn)是使管子工作在占空比可調(diào)的開(kāi)關(guān)狀態(tài)，提高使用效率實(shí)現(xiàn)電機(jī)轉(zhuǎn)速的微調(diào)。并且保證了可以簡(jiǎn)單的方式實(shí)現(xiàn)方向控制。綜合以上三種方案，選擇方案三。1.5飛行姿態(tài)控制方案論證 方案一： 十字飛行方式。四軸的四個(gè)電機(jī)以十字的方式排列，x軸和y軸成直角，調(diào)整俯仰角和翻滾角的時(shí)候分開(kāi)調(diào)整，角度融合簡(jiǎn)單，適合初學(xué)者，能明確頭

14、尾，飛行時(shí)機(jī)體動(dòng)作精準(zhǔn)，飛控起來(lái)也容易。方案二：X行飛行方式。四軸的四個(gè)電機(jī)以X字的方式排列，靈活性和可調(diào)性較高，調(diào)整的時(shí)候應(yīng)該相鄰兩個(gè)融合調(diào)節(jié)，融合復(fù)雜。X型飛行方式非常自由靈活，旋轉(zhuǎn)方式多樣，可以花樣飛行，也可以做出很多高難度動(dòng)作，但是控制上相對(duì)比較困難。 綜合以上兩種方案鑒于我們是初次嘗試，所以選擇了方案一。1.6 MPU9150模塊的方案論證 方案一： 光纖陀螺儀。光纖陀螺儀是以光導(dǎo)纖維線圈為基礎(chǔ)的敏感元件，由激光二極管發(fā)射出的光線朝兩個(gè)方向沿光導(dǎo)纖維傳播。光傳播路徑的變化，決定了敏感元件的角位移。光纖陀螺儀壽命長(zhǎng)，動(dòng)態(tài)范圍大，瞬時(shí)啟動(dòng)，結(jié)構(gòu)簡(jiǎn)單，尺寸小，重量輕，但是成本較高，鑒于我們

15、這是初次嘗試，需要多次實(shí)驗(yàn)，破壞較大。方案(fng n)二： MPU-9150為系統(tǒng)級(jí)封裝(SiP：System in Package)產(chǎn)品，整合了兩個(gè)芯片：一為MPU-6050，含三軸陀螺儀、三軸加速器，內(nèi)建可處理復(fù)雜(fz)之九軸運(yùn)動(dòng)感測(cè)融合算法的數(shù)字運(yùn)動(dòng)感測(cè)處理器(DMP：Digital Motion Processor)；另一為AK8975，是三軸數(shù)字電子羅盤(pán)。MPU-9150所含之九軸運(yùn)動(dòng)感測(cè)融合算法，可連接所有內(nèi)部的傳感器，收集整組感測(cè)數(shù)據(jù)。此產(chǎn)品為4x4x1mm的LGA包裝，為MPU-6050整合性六軸運(yùn)動(dòng)感測(cè)追蹤組件的兼容升級(jí)版，提供簡(jiǎn)單的升級(jí)路徑，易于安裝在空間受限的板子上。

16、綜合以上(yshng)兩種方案，我們選擇了方案二二、系統(tǒng)理論分析與計(jì)算 式1-1式中為四旋翼飛行器在導(dǎo)航坐標(biāo)系下的線位移，為運(yùn)動(dòng)加速度，m為飛行器質(zhì)量，分別為機(jī)體的偏航角、俯仰角和橫滾角，為旋翼面中心到四旋翼飛行器質(zhì)心的距離， 為軸向慣性主矩。該動(dòng)力學(xué)模型對(duì)四旋翼飛行器的真實(shí)飛行狀態(tài)進(jìn)行了合理的簡(jiǎn)化，忽略了空氣阻力等對(duì)系統(tǒng)運(yùn)行影響較小的參數(shù)，使得飛行控制算法更加簡(jiǎn)潔。2.2 PID控制算法結(jié)構(gòu)(jigu)分析在動(dòng)力學(xué)模型的基礎(chǔ)(jch)上，將小型四旋翼飛行器實(shí)時(shí)控制算法分為兩個(gè)控制回路，即位置控制回路和姿態(tài)控制回路。算法結(jié)構(gòu)如圖B-1所示。給定位置位置控制姿 態(tài)控 制電機(jī)控制飛行器機(jī)體姿態(tài)控制

17、回路位置控制回路 圖2-1 四旋翼飛行器控制算法結(jié)構(gòu)圖使用經(jīng)典PID控制算法實(shí)現(xiàn)位置控制回路和姿態(tài)控制回路。PID算法簡(jiǎn)單(jindn)可靠，理論體系完備，而且在長(zhǎng)期的應(yīng)用過(guò)程中積攢了大量的使用經(jīng)驗(yàn)，在飛行器位置和姿態(tài)控制應(yīng)用中具有良好的控制效果和較強(qiáng)的魯棒性，能提供控制量的較優(yōu)解。控制回路包含了三個(gè)控制量，因此設(shè)計(jì)3個(gè)獨(dú)立的PID控制器對(duì)位移進(jìn)行控制。根據(jù)PID控制器的原理，設(shè)分別為比例項(xiàng)、積分項(xiàng)和微分項(xiàng)系數(shù)， 式1-2其中，為航姿參考系統(tǒng)測(cè)量到的加速度積分得到的位移量。姿態(tài)控制回路的作用是控制四旋翼飛行器的飛行姿態(tài)，使其實(shí)際(shj)姿態(tài)與設(shè)定的姿態(tài)一致。姿態(tài)控制回路有偏航角、俯仰角和橫滾

18、角三個(gè)控制量，在此忽略三個(gè)通道之間的耦合效應(yīng)，設(shè)計(jì)3個(gè)獨(dú)立的PID控制器對(duì)每個(gè)量進(jìn)行獨(dú)立控制。 式1-3根據(jù)(gnj)PID控制器的原理，設(shè)分別為比例項(xiàng)、積分項(xiàng)和微分項(xiàng)系數(shù)(xsh)，有控制器方程如B-4。 式1-4其中比例項(xiàng)系數(shù)為3.3積分項(xiàng)系數(shù)為0.14微分項(xiàng)系數(shù)為3.2。三、電路與程序設(shè)計(jì)1、電路的設(shè)計(jì)（1）系統(tǒng)總體框圖系統(tǒng)以R5F100LE單片機(jī)為核心，主要包括電源模塊、電機(jī)驅(qū)動(dòng)模塊、飛行控制模塊、傳感器檢測(cè)等功能模塊，該系統(tǒng)硬件結(jié)構(gòu)框圖如圖所示。圖1 系統(tǒng)硬件結(jié)構(gòu)圖（2）飛行(fixng)控制系統(tǒng)(xtng)實(shí)物圖與電路(dinl)原理圖1、R5F100LE單片機(jī)飛行控制系統(tǒng)實(shí)物圖圖

19、2 2、四軸飛行器控制模型圖3 四軸飛行器控制(kngzh)模型 3、飛行(fixng)控制系統(tǒng)(xtng)系統(tǒng)電路圖2 飛行控制系統(tǒng)系統(tǒng)電路（3）5V電源(dinyun)系統(tǒng)框圖(kungt)與電路原理圖1、系統(tǒng)(xtng)實(shí)物圖圖1 系統(tǒng)實(shí)物圖2、系統(tǒng)電路圖2 電源子系統(tǒng)電路（4）總電源(dinyun)電源(dinyun)由一塊11.1V 2200ma的鋰電池（重量約為166克）供電，在由電調(diào)降壓給系統(tǒng)中的各個(gè)模塊供5v電壓并給電機(jī)提供電流，這樣可滿(mǎn)足(mnz)可滿(mǎn)足各個(gè)小系統(tǒng)的電源要求。（5）MPU9150模塊 1、 MPU9150模塊 2、MPU9150實(shí)物圖2、程序(chngx)的設(shè)

20、計(jì)本系統(tǒng)以瑞薩MCU為核心，采用C語(yǔ)言對(duì)單片機(jī)進(jìn)行(jnxng)編程。主程序主要起導(dǎo)向和決策的作用，系統(tǒng)的控制總流程圖如圖所示。系統(tǒng)包括延時(shí)子程序，電機(jī)轉(zhuǎn)速控制子程序，檢測(cè)子程序，副翼子程序。系統(tǒng)控制的總流程圖如圖所示。部分(b fen)程序清單見(jiàn)附錄A。按鍵開(kāi)始系統(tǒng)初始化上升定高校正姿態(tài)前行紅外傳感器1是否檢測(cè)到黑線？計(jì)黑線值減速降落YN紅外傳感器2是否檢測(cè)到黑線？黑線值=3？尋點(diǎn)YNY系統(tǒng)(xtng)主程序流程圖四、測(cè)試方案(fng n)與結(jié)果分析4.1 測(cè)試(csh)設(shè)備方案 模擬區(qū)域：按題目要求設(shè)計(jì)。設(shè)備：秒表、卷尺。測(cè)試環(huán)境為室內(nèi)。4.2 測(cè)試(csh)結(jié)果使系統(tǒng)運(yùn)行，參照題目的各項(xiàng)

21、要求進(jìn)行(jnxng)多次實(shí)際定量測(cè)量，得到該系統(tǒng)的測(cè)量結(jié)果如表表B-1所示。表B-1 測(cè)試(csh)功能、測(cè)試工具及實(shí)現(xiàn)的情況表具體功能要求實(shí)際測(cè)試結(jié)果基本部分四旋翼自主飛行器一鍵式啟動(dòng)，從A區(qū)飛向B區(qū)，在B區(qū)降落并停機(jī)；飛行時(shí)間不大于45s可以滿(mǎn)足，飛行時(shí)間為20s四旋翼自主飛行器一鍵式啟動(dòng)，從B區(qū)飛向A區(qū)，在A區(qū)降落并停機(jī)；飛行時(shí)間不大于45s可以滿(mǎn)足要求，飛行時(shí)間為17s發(fā)揮部分飛行器擺放在A區(qū)，飛行器下面擺放一薄鐵片，一鍵式啟動(dòng)，飛行器拾取薄鐵片并起飛未進(jìn)行測(cè)試。飛行器攜帶薄鐵片從示高線上方飛向B區(qū)，并在空中將薄鐵片投放到B區(qū)；飛行器從示高線上方返回A區(qū)，在A區(qū)降落并停機(jī)未進(jìn)行測(cè)試。

22、以上往返飛行時(shí)間不大于30s其他經(jīng)測(cè)試，系統(tǒng)基本可以滿(mǎn)足基本要求和部分發(fā)揮部分，并具有穩(wěn)定前行的特點(diǎn)。五、 總結(jié)(zngji)本次設(shè)計(jì)按照題目(tm)要求，采用模塊化的硬件和軟件設(shè)計(jì)方法，運(yùn)用PID控制算法，成功地實(shí)現(xiàn)了飛行器尋黑帶自主飛行并定點(diǎn)降落的要求。具有飛行穩(wěn)定、運(yùn)動(dòng)迅速、接收信號(hào)靈敏等特點(diǎn)。完成了題目基本部分和部分發(fā)揮部分的要求。六、參考文獻(xiàn)1譚浩強(qiáng).C語(yǔ)言程序設(shè)計(jì)(chn x sh j)M.北京:清華大學(xué)出版社,20122李曉林.單片機(jī)原理與接口技術(shù).電子工業(yè)出版社,20103黃智偉.全國(guó)電子設(shè)計(jì)大賽設(shè)計(jì)競(jìng)賽訓(xùn)練教材.電子工業(yè)出版,20104房玉明,杭柏林.基于單片機(jī)的步進(jìn)電機(jī)開(kāi)環(huán)

23、控制系統(tǒng)J.電機(jī)與控制應(yīng)用20065袁任光,張偉武.電動(dòng)機(jī)控制電路選用與258實(shí)例M.北京:機(jī)械工業(yè)出版社,2005.6 全國(guó)大學(xué)生電子設(shè)計(jì)競(jìng)賽組委會(huì). 第四屆（1999年）7黃智偉.全國(guó)大學(xué)生電子設(shè)計(jì)競(jìng)賽EDA技術(shù)教學(xué)方法研究J. 南華教育2003年第2期七、附錄1、儀器設(shè)備設(shè)備清單序號(hào)名稱(chēng)型號(hào)數(shù)量備注1四旋翼飛行器F3312MCUR5F100LEA116位微處理器3三軸加速器MPU9150116位數(shù)據(jù)輸出4電源11V1鋰電池2、程序清單飛行器控制器端部分(b fen)程序#include #include delay.h#include WSmotor.h#include key.h#in

24、clude sys.h#include time.h#include NRF24l01.h#include /Keil library u32 tm0,tm1,sec1,sec2,sec3;float gyro,angle_n,dt_old,dt_new,dt_now,angle_n_all,count_gyro,GX,GY,GZ;u8 k,k1,k2,k3,k4;u8 left_lean,right_lean,left_add_speed,right_add_speed,front_lean,back_lean,front_add_speed,back_add_speed,left_lean

25、_10flag,right_lean_10flag,front_lean_10flag,back_lean_10flag,left_lean_20flag,right_lean_20flag,front_lean_20flag,back_lean_20flag,left_lean_30flag,right_lean_30flag,front_lean_30flag,back_lean_30flag;#define Kp 2.0f #define Ki 0.005f #define halfT 0.1f float q0 = 1, q1 = 0, q2 = 0, q3 = 0; float ex

26、Int = 0, eyInt = 0, ezInt = 0; S_FLOAT_ANGLE Q_ANGLE;void delay(u16 i)u8 j;for(; i != 0; i-)for(j = 110; j 0; j-);extern void Timerx_Init(u16 arr,u16 psc);extern void motor_Init(void);/初始化GPIO_InitTypeDef GPIO_InitStructure;ErrorStatus HSEStartUpStatus;#define uchar unsigned char#define uint unsigne

27、d int#define OSS 0 /BMP085使用(shyng)/L3G4200D內(nèi)部(nib)寄存器#define CTRL_REG1 0 x20#define CTRL_REG2 0 x21#define CTRL_REG3 0 x22#define CTRL_REG4 0 x23#define CTRL_REG5 0 x24#define OUT_X_L 0 x28#define OUT_X_H 0 x29#define OUT_Y_L 0 x2A#define OUT_Y_H 0 x2B#define OUT_Z_L 0 x2C#define OUT_Z_H 0 x2D/定義器件

28、在IIC總線(zn xin)中的從地址,根據(jù)ALT ADDRESS地址引腳不同修改#defineHMC5883L_Addr 0 x3C/磁場(chǎng)傳感器器件(qjin)地址#defineADXL345_Addr 0 xA6/加速度傳感器器件(qjin)地址#defineBMP085_Addr 0 xee/氣壓傳感器器件地址#defineL3G4200_Addr 0 xD2/陀螺儀傳感器器件地址unsigned char BUF8; /接收數(shù)據(jù)緩存區(qū)char test=0; int x,y;float angle;uchar ge,shi,bai,qian,wan,shiwan; /顯示變量int A

29、_X,A_Y,A_Z;short T_X,T_Y,T_Z;short data_xyz3;float Roll,Pitch,Q,T,K;/*BMP085使用short ac1;short ac2; short ac3; unsigned short ac4;unsigned short ac5;unsigned short ac6;short b1; short b2;short mb;short mc;short md;long temperature;long pressure;/*模擬IIC端口輸出輸入定義*/#define SCL_H GPIOB-BSRR = GPIO_Pin_6#define SCL_L GPIOB-BRR = GPIO_Pin_6 #define SDA_H GPIOB-BSRR = GPIO_Pin_7#define SDA_L GPIOB-BRR = GPIO_Pin_7#define SCL_read GPIOB-IDR & GPIO_Pin_6#define SDA_read GPIOB-IDR & GPIO_Pin_7void RCC