簡(jiǎn)易旋轉(zhuǎn)倒立擺及控制裝置

1、2013年全國(guó)大學(xué)生電子設(shè)計(jì)競(jìng)賽簡(jiǎn)易旋轉(zhuǎn)倒立擺及控制裝置（C題）【本科組】2013年9月7日摘 要 本簡(jiǎn)易旋轉(zhuǎn)倒立擺控制系統(tǒng)采用STC89C52單片機(jī)作為主控芯片，BTS7960模塊作為電機(jī)驅(qū)動(dòng)，用MPU-6050角度傳感器檢測(cè)擺桿位置，單片機(jī)用PID算法控制直流減速電機(jī)PWM信號(hào)精確控制電機(jī)的轉(zhuǎn)速和角度，從而實(shí)現(xiàn)驅(qū)動(dòng)電機(jī)帶動(dòng)旋轉(zhuǎn)臂做往復(fù)旋轉(zhuǎn)使擺桿擺動(dòng)，在規(guī)定時(shí)間內(nèi)實(shí)現(xiàn)擺角超過60，擺桿從自然下垂?fàn)顟B(tài)完成圓周運(yùn)動(dòng)和從165起始位置保持倒立，并且使用液晶屏顯示擺桿角度和模式。本倒立擺系統(tǒng)用+12V開關(guān)電源給減速電機(jī)供電，用+5V穩(wěn)壓電源給單片機(jī)供電。最后的測(cè)試表明，本系統(tǒng)基本達(dá)到了基本要求。關(guān)鍵

2、詞：?jiǎn)纹瑱C(jī)，角度傳感器，直流減速電機(jī)，PID，液晶顯示 目 錄1系統(tǒng)方案11.1 主控芯片的選擇11.2 角度檢測(cè)模塊選擇21.3 電機(jī)的選擇21.4 電機(jī)驅(qū)動(dòng)的選擇22系統(tǒng)理論分析與計(jì)算32.1 控制算法的分析32.2 擺桿角度的計(jì)算43電路與程序設(shè)計(jì)43.1電路的設(shè)計(jì)4系統(tǒng)總體框圖43.1.2各模塊電路原理圖43.2程序的設(shè)計(jì)7程序功能描述與設(shè)計(jì)思路7程序流程圖84測(cè)試方案與測(cè)試結(jié)果84.1測(cè)試方案84.2 測(cè)試條件與儀器94.3 測(cè)試結(jié)果及分析9測(cè)試結(jié)果(數(shù)據(jù))9測(cè)試分析與結(jié)論9附錄1：電路原理圖10附錄2：源程序11簡(jiǎn)易旋轉(zhuǎn)倒立擺及控制裝置（C題）【本科組】1系統(tǒng)方案本題要求設(shè)計(jì)并制作

3、一個(gè)簡(jiǎn)易旋轉(zhuǎn)倒立擺控制系統(tǒng)，能實(shí)現(xiàn)擺桿擺過一定角度和做完成圓周運(yùn)動(dòng)，保持倒立的基本要求部分。發(fā)揮部分要求實(shí)現(xiàn)擺桿從自然狀態(tài)擺起保持倒立并有一定額抗干擾能力，并且在擺桿保持倒立狀態(tài)下旋臂可以單方向轉(zhuǎn)過360以上。 圖1-1旋轉(zhuǎn)倒立擺模型圖本系統(tǒng)利用角度傳感器檢測(cè)單擺擺動(dòng)角度，利用單片機(jī)接收信號(hào)并控制直流減速電機(jī)來控制旋臂轉(zhuǎn)動(dòng)從而控制擺桿擺動(dòng)。本系統(tǒng)主要由控制器模塊、角度檢測(cè)模塊、電機(jī)模塊、電機(jī)驅(qū)動(dòng)模塊、電源模塊組成，下面分別論證這幾個(gè)模塊的選擇。1.1 主控芯片的選擇方案一：采用凌陽公司的SPCE061A單片機(jī)作為控制器的方案。該單片機(jī)I/O資源豐富，但是價(jià)格昂貴。方案二：使用FPGA，CPLD

4、等大規(guī)?？删幊踢壿嬁刂破骷?，其時(shí)鐘頻率高，運(yùn)算速度很快，但不適合于該題目。 方案三：采用ATMEL公司的STC89C52。52單片機(jī)價(jià)格便宜，應(yīng)用廣泛，其功能足以實(shí)現(xiàn)本設(shè)計(jì)要求。綜合考慮，我們選擇方案三。1.2 角度檢測(cè)模塊選擇方案一：使用電位器作為角度傳感器，由于不同角度輸出的電阻值不同，通過AD采樣電阻兩端電壓，計(jì)算得到角度。 但是，對(duì)于一般的電位器，線性度較差，而對(duì)于線性度較好的電位器，如22HP-10等，價(jià)格較高。方案二：采用OMRON公司的E6B2-CWZ6C編碼器，數(shù)字信號(hào)輸出，采集方便，價(jià)格昂貴。方案三：采用MPU6050加速度傳感器，數(shù)字輸出（I2C/SPI ）、低功耗、緊湊型

5、電容式微機(jī)械加速度計(jì)，具有輸出精確，體積小，工作可靠，各種標(biāo)識(shí)清晰，擴(kuò)展性好等特點(diǎn)。綜合以上三種方案，選擇方案三。1.3 電機(jī)的選擇方案一：使用伺服電機(jī)作為執(zhí)行元件，運(yùn)行精確，能高速制動(dòng)，慣量小，適合閉環(huán)控制。但價(jià)格昂貴，編程復(fù)雜。 方案二：采用57步步進(jìn)電機(jī)控制平板的運(yùn)動(dòng)，最小步進(jìn)角為1.8度，因此能實(shí)現(xiàn)轉(zhuǎn)動(dòng)的精確控制，但步進(jìn)電機(jī)的震動(dòng)動(dòng)不利于擺桿的倒立平衡。方案三：采用直流減速電機(jī)控制平板，直流減速電機(jī)力矩大，轉(zhuǎn)動(dòng)速度快，反應(yīng)靈敏，易于及時(shí)控制。綜合考慮采用方案三。1.4 電機(jī)驅(qū)動(dòng)的選擇方案一：采用普通三極管作H橋式驅(qū)動(dòng)電路。三極管作為一個(gè)典型的直流電機(jī)驅(qū)動(dòng)電路。電路得名于“H橋式驅(qū)動(dòng)電路

6、”是因?yàn)樗男螤羁崴谱帜窰。4個(gè)三極管組成H的4條垂直腿，而電機(jī)就是H中的橫杠。H橋式電機(jī)驅(qū)動(dòng)電路包括4個(gè)三極管和一個(gè)電機(jī)。要使電機(jī)運(yùn)轉(zhuǎn)，必須導(dǎo)通對(duì)角線上的一對(duì)三極管。根據(jù)不同三極管對(duì)的導(dǎo)通情況，電流可能會(huì)從左至右或從右至左流過電機(jī)，從而控制電機(jī)的轉(zhuǎn)向。此H橋電路結(jié)構(gòu)簡(jiǎn)單，容易實(shí)現(xiàn)，但是驅(qū)動(dòng)能力較小三極管容易發(fā)熱，所以不采用此方案。方案二：采用L298N驅(qū)動(dòng)電路。L298N是SGS公司的產(chǎn)品，內(nèi)部包含4通道邏輯驅(qū)動(dòng)電路，是一種兩相和四相電機(jī)的專用驅(qū)動(dòng)器，即內(nèi)部含二個(gè)H橋的高電壓大電流雙全橋式驅(qū)動(dòng)器，接收標(biāo)準(zhǔn)TTL邏輯電平信號(hào)，可驅(qū)動(dòng)46V、2A一下的電機(jī)。PWM配合橋是驅(qū)動(dòng)電路L298N，實(shí)現(xiàn)

7、直流電機(jī)調(diào)速，非常簡(jiǎn)單，調(diào)速范圍大，但是L298N在驅(qū)動(dòng)大功率電機(jī)時(shí)發(fā)熱較嚴(yán)重容易燒壞芯片，所以不采用此方案。方案三：采用BTS7960驅(qū)動(dòng)電路。BTS7960是應(yīng)用于電機(jī)驅(qū)動(dòng)的大電流半橋式高集成芯片，它帶有一個(gè)P夠到的高邊MOSFET、一個(gè)N溝道的底邊MOSGET和一個(gè)驅(qū)動(dòng)IC。P溝道高邊開關(guān)省去了電荷泵的需求，因而減小了EMI. 集成的驅(qū)動(dòng)IC具有邏輯電平輸入、電流診斷、斜率調(diào)節(jié)、死區(qū)時(shí)間產(chǎn)生和過溫、過壓、欠壓、過流及短路保護(hù)的功能。BTS7960通態(tài)電阻典型值為16m，驅(qū)動(dòng)電流可達(dá)43A。此方案能實(shí)現(xiàn)直流電機(jī)調(diào)速，調(diào)速范圍大，驅(qū)動(dòng)能力強(qiáng)且不易發(fā)熱，所以采用BTS7960作為此電路的驅(qū)動(dòng)電

8、路。2系統(tǒng)理論分析與計(jì)算2.1 控制算法的分析 倒立擺系統(tǒng)是一個(gè)比較復(fù)雜的，帶有快速、高階次、多變量、嚴(yán)重非線性絕對(duì)不穩(wěn)定和非最小相位系統(tǒng)的機(jī)電系統(tǒng)。PID 控制是由反饋系統(tǒng)偏差的比例(P)、積分(I),微分(D)的線性組合構(gòu)成的反饋控制律，由于它具有原理簡(jiǎn)單、直觀易懂、易于工程實(shí)現(xiàn)、魯棒性強(qiáng)等一系列優(yōu)點(diǎn)，多年以來它一直是工業(yè)過程控制中應(yīng)用最廣泛的一類控制算法。其輸入與輸出的關(guān)系為： 圖2-1a) 比例環(huán)節(jié)：成比例地反映控制系統(tǒng)的偏差信號(hào)e(t)，偏差一旦產(chǎn)生，控制器立即產(chǎn)生控制作用，以減小偏差。b) 積分環(huán)節(jié)：主要用于消除靜差，提高系統(tǒng)的無差度。積分作用的強(qiáng)弱取決于積分時(shí)間常數(shù)T，T越大，積

9、分作用越弱，反之則越強(qiáng)。c) 微分環(huán)節(jié)：反映偏差信號(hào)的變化趨勢(shì)，并能在偏差信號(hào)變得太大之前，在系統(tǒng)中引入一個(gè)有效的早期修正信號(hào)，從而加快系統(tǒng)的動(dòng)作速度，減少調(diào)節(jié)時(shí)間。根據(jù)經(jīng)驗(yàn)先將控制器的參數(shù)放在某一數(shù)值上，直接在閉環(huán)控制系統(tǒng)中，根據(jù)實(shí)際現(xiàn)象調(diào)節(jié)各個(gè)參數(shù)，調(diào)出最優(yōu)控制。2.2 擺桿角度的計(jì)算 角度傳感器內(nèi)部集成了加速度傳感器和陀螺儀，可以用兩種方法算出擺桿與豎直方向的夾角。用加速度傳感器時(shí)，通過重力加速度算出其在x軸和y軸方向重力加速度分量，從而計(jì)算出擺桿與豎直方向的夾角。用陀螺儀時(shí)，通過檢測(cè)陀螺儀在y軸方向的離心力對(duì)周圍空氣的壓力，得出擺桿旋轉(zhuǎn)的角速度，再對(duì)角速度進(jìn)行積分，從而得出擺桿擺過的角

10、度。3電路與程序設(shè)計(jì)3.1電路的設(shè)計(jì)系統(tǒng)總體框圖單片機(jī)對(duì)角度傳感器檢測(cè)到的角度進(jìn)行PID運(yùn)算，通過控制直流電機(jī)占空比從而控制旋臂的轉(zhuǎn)速，進(jìn)而控制擺臂旋轉(zhuǎn)。系統(tǒng)總體框圖如圖3-1所示： BTS驅(qū)動(dòng) 直流電機(jī)旋臂 擺桿單 片 機(jī)角度傳感器 液晶屏顯示 圖3-1 系統(tǒng)總體框圖3.1.2各模塊電路原理圖 本系統(tǒng)主要包括主控制模塊、角度傳感器模塊、電機(jī)及其驅(qū)動(dòng)模塊、電源模塊、液晶顯示模塊。1、 電機(jī)及其驅(qū)動(dòng)模塊電路圖 圖3-2 BTS電機(jī)驅(qū)動(dòng)電路圖BTS7960B是一款集成度很高的大電流半橋驅(qū)動(dòng)芯片，該芯片內(nèi)部集成了一個(gè)P溝道的高端MOSFET和一個(gè)N溝道的低端MOSFET，由于高端采用P溝道MOSFE

11、T，因此避免了充電泵的電磁干擾，提高了EMC能力。驅(qū)動(dòng)電流可直接由輸入邏輯電平控制，簡(jiǎn)化了電路設(shè)計(jì)，并提高了電路可靠性。持續(xù)的漏極電流高達(dá)40A，能允許PWM的頻率從0Hz到25kHz之間任意變化，滿足目標(biāo)電機(jī)的各種驅(qū)動(dòng)需求。通過狀態(tài)標(biāo)志位( IS位)，能實(shí)現(xiàn)電機(jī)狀態(tài)診斷和電流采樣，并具有各種保護(hù)功能?？赏ㄟ^外部電阻調(diào)節(jié)內(nèi)部MOSFET 開關(guān)速率，以獲得最優(yōu)EMI。本系統(tǒng)采用四片BTS7960B做成一個(gè)驅(qū)動(dòng)電路以最小的板極空間為高電流的PWM調(diào)速電機(jī)提供了一種較優(yōu)的解決方案。2、傳感器模塊電路圖 圖3-3 傳感器模塊電路圖MPU-60X0是全球首例9軸運(yùn)動(dòng)處理傳感器。它集成了3軸MEMS陀螺儀

12、，3軸MEMs加速度計(jì)以及一個(gè)可擴(kuò)展的數(shù)字運(yùn)動(dòng)處理器DMP（DigitalMotionProcessor），可用I2C接口連接一個(gè)第三方的數(shù)字傳感器，比如磁力計(jì)。擴(kuò)展之后就可以通過其I2C或SPI接口輸出一個(gè)9軸的信號(hào)（SPI接口僅在MPU-6000可用）。MPU-60X0對(duì)陀螺儀和加速度計(jì)分別用了三個(gè)16位的ADC，將其測(cè)量的模擬量轉(zhuǎn)化為可輸出的數(shù)字量。為了精確跟蹤快速和慢速的運(yùn)動(dòng)，傳感器的測(cè)量范圍都是用戶可控的，陀螺儀可測(cè)范圍為250，500，1000，2000/秒（dps），加速度計(jì)可測(cè)范圍為2，4，8，16g。本系統(tǒng)采用此角度傳感器檢測(cè)擺桿轉(zhuǎn)過的角度。 3、液晶顯示模塊電路圖 圖3-4

13、 液晶顯示模塊電路圖 用1602LCD液晶進(jìn)行顯示。LCD由于顯示清晰，內(nèi)容精確、顯示信息較多且使用方便、速度快。結(jié)合設(shè)計(jì)方案要求實(shí)時(shí)的顯示擺桿角度和控制模式。4、單片機(jī)控制模塊電路圖 圖3-5 單片機(jī)控制模塊電路圖 STC89C52是STC公司生產(chǎn)的一種低功耗、高性能CMOS8位微控制器，具有 8K 在系統(tǒng)可編程Flash存儲(chǔ)器。具有以下標(biāo)準(zhǔn)功能： 8k字節(jié)Flash，512字節(jié)RAM， 32 位I/O 口線，看門狗定時(shí)器，內(nèi)置4KB EEPROM，MAX810復(fù)位電路，3個(gè)16 位定時(shí)器/計(jì)數(shù)器，4個(gè)外部中斷，一個(gè)7向量4級(jí)中斷結(jié)構(gòu)（兼容傳統(tǒng)51的5向量2級(jí)中斷結(jié)構(gòu)），全雙工串行口。另外

14、STC89X52 可降至0Hz 靜態(tài)邏輯操作，支持2種軟件可選擇節(jié)電模式。空閑模式下，CPU 停止工作，允許RAM、定時(shí)器/計(jì)數(shù)器、串口、中斷繼續(xù)工作。掉電保護(hù)方式下，RAM內(nèi)容被保存，振蕩器被凍結(jié)，單片機(jī)一切工作停止，直到下一個(gè)中斷或硬件復(fù)位為止。最高運(yùn)作頻率35MHz，6T/12T可選。本系統(tǒng)采用AT89C52作為主控芯片，就采集到的角度值進(jìn)行PID運(yùn)算，控制PWM波的占空比而控制電機(jī)轉(zhuǎn)速，從而控制擺桿擺動(dòng)角度。3.2程序的設(shè)計(jì)程序功能描述與設(shè)計(jì)思路 通過單片機(jī)對(duì)角度傳感器檢測(cè)到的擺桿角度進(jìn)行PID計(jì)算，進(jìn)而控制輸出PWM波占空比，調(diào)節(jié)電機(jī)轉(zhuǎn)速，從而控制旋臂轉(zhuǎn)速?gòu)亩刂茢[桿擺動(dòng)。通過三個(gè)按

15、鍵設(shè)置三個(gè)模式，模式一實(shí)現(xiàn)擺桿盡快擺過60，模式二實(shí)現(xiàn)擺桿迅速擺過一周，模式三實(shí)現(xiàn)對(duì)擺桿倒立平衡的控制。另外用液晶屏顯示擺桿的角度和當(dāng)前使用的控制模式。程序流程圖 開始 初始化 按鍵1 按鍵2 按鍵3 PID算法1 PID算法2 測(cè)角度 驅(qū)動(dòng)電機(jī) 驅(qū)動(dòng)電機(jī) PID算法3 保持在80 擺一周 驅(qū)動(dòng)電機(jī) 結(jié)束 結(jié)束 保持倒立 是 否 結(jié)束 圖3-6 程序流程圖4測(cè)試方案與測(cè)試結(jié)果4.1測(cè)試方案1、 硬件測(cè)試 用萬用變測(cè)量開關(guān)電源輸出電壓為+10.4V，測(cè)得穩(wěn)壓電源電壓為+5V,電機(jī)驅(qū)動(dòng)模塊輸出電壓為+10.2V，硬件電路工作正常。2、硬件軟件聯(lián)調(diào)用導(dǎo)線將各模塊連接起來，將程序下載進(jìn)單片機(jī)，通電測(cè)試

16、，如期實(shí)現(xiàn)了基本部分的要求。4.2 測(cè)試條件與儀器測(cè)試條件：檢查多次，仿真電路和硬件電路必須與系統(tǒng)原理圖完全相同，并且檢查無誤，硬件電路保證無虛焊。測(cè)試儀器：萬用表，量角器。4.3 測(cè)試結(jié)果及分析測(cè)試結(jié)果(數(shù)據(jù))1、 基本要求（1）部分測(cè)試數(shù)據(jù)： 表4-1 擺角達(dá)60以上數(shù)據(jù) 次數(shù) 1 2 3 4 5 時(shí)間(s) 1.02 1.09 1.11 1.01 0.87 擺動(dòng)角度 72 95 110 83 752、 基本要求（2）部分測(cè)試數(shù)據(jù)： 表4-2 完成圓周運(yùn)動(dòng)數(shù)據(jù) 次數(shù) 1 2 3 4 5 時(shí)間(s) 0.53 0.58 0.63 0.90 0.713、 基本要求（3）部分測(cè)試數(shù)據(jù)： 表4-3

17、 保持?jǐn)[桿倒立數(shù)據(jù) 次數(shù) 1 2 3 4 5 拉起位置 165 170 170 175 180保持時(shí)間(s) 0.64 1.12 1.21 2.05 2.08 旋臂轉(zhuǎn)角 90 135 138 190 187測(cè)試分析與結(jié)論根據(jù)上述測(cè)試數(shù)據(jù)，本系統(tǒng)可以得出以下結(jié)論：1、 擺桿從自然下垂?fàn)顟B(tài)開始，驅(qū)動(dòng)電機(jī)帶動(dòng)旋轉(zhuǎn)臂做往復(fù)旋轉(zhuǎn)，能夠使擺角迅速超過60。2、 擺桿從自然下垂?fàn)顟B(tài)起，盡快增大擺桿擺動(dòng)幅度，能迅速完成圓周運(yùn)動(dòng)。3、擺桿從自然下垂?fàn)顟B(tài)下，外力拉起擺桿至180位置，撤除外力同時(shí)啟動(dòng)旋轉(zhuǎn)臂使擺桿保持倒立狀態(tài)能達(dá)到兩秒左右，期間旋轉(zhuǎn)臂轉(zhuǎn)動(dòng)角度不大于90。綜上所述，本設(shè)計(jì)達(dá)到設(shè)計(jì)基本要求的大部分，但控

18、制精度和穩(wěn)定性還需要進(jìn)一步完善。附錄1：電路原理圖附錄2：源程序#include#include#include#include#include#define uchar unsigned char#define uint unsigned int/ 定義MPU6050內(nèi)部地址#define SMPLRT_DIV 0x19 /陀螺儀采樣率，典型值：x07(125Hz)#define CONFIG 0x1A /低通濾波頻率，典型值：x06(5Hz)#define GYRO_CONFIG 0x1B /陀螺儀自檢及測(cè)量范圍，典型值：x18(不自檢，deg/s)#define ACCEL_CONFIG

19、 0x1C /加速計(jì)自檢、測(cè)量范圍及高通濾波頻率，典型值：x01(不自檢，G，Hz)#define ACCEL_XOUT_H 0x3B#define ACCEL_XOUT_L 0x3C#define ACCEL_YOUT_H 0x3D#define ACCEL_YOUT_L 0x3E#define ACCEL_ZOUT_H 0x3F#define ACCEL_ZOUT_L 0x40#define TEMP_OUT_H 0x41#define TEMP_OUT_L 0x42#define GYRO_XOUT_H 0x43#define GYRO_XOUT_L 0x44 #define GYRO_

20、YOUT_H 0x45#define GYRO_YOUT_L 0x46#define GYRO_ZOUT_H 0x47#define GYRO_ZOUT_L 0x48#define PWR_MGMT_1 0x6B /電源管理，典型值：x00(正常啟用)#define WHO_AM_I 0x75 /IIC地址寄存器(默認(rèn)數(shù)值x68，只讀)#define SlaveAddress 0xD0 /IIC寫入時(shí)的地址字節(jié)數(shù)據(jù)，+1為讀取sbit key1=P14;sbit key2=P15;sbit key3=P16;sbit PWM1=P22;sbit PWM2=P23;uchar mode;/模式/

21、uchar z,zz;/制動(dòng)變量uint m,a;/PWM波占空比double angle;/角度bit dir;/正轉(zhuǎn)與反轉(zhuǎn)標(biāo)志位typedef struct PID/結(jié)構(gòu)體int SetPoint; /設(shè)定目標(biāo)Desired Value long SumError; /誤差累計(jì)double Proportion; /比例常數(shù)Proportional Const double Derivative; /微分常數(shù)Derivative Const int LastError; /Error-1 int PrevError; /Error-2 PID;static PID sPID;static

22、PID *sptr = &sPID;void IncPIDInit(void)/PID參數(shù)初始化sptr-SumError = 0;sptr-LastError = 0; /Error-1sptr-PrevError = 0; /Error-2sptr-Proportion = 0.5; /比例常數(shù)Proportional Const sptr-Derivative = 5; /微分常數(shù)Derivative Const sptr-SetPoint = 0; int IncPIDCalc(int NextPoint) register int iError, iIncpid; /當(dāng)前誤差iErr

23、or = sptr-SetPoint - NextPoint; /增量計(jì)算iIncpid = sptr-Proportion * iError /Ek項(xiàng)+ sptr-Derivative * sptr-PrevError; /Ek項(xiàng)/存儲(chǔ)誤差，用于下次計(jì)算sptr-PrevError = sptr-LastError; sptr-LastError = iError; /返回增量值return(iIncpid); void write_MUP6050_single(uchar address,uchar date)start_I2C();write_byte_I2C(SlaveAddress)

24、;/發(fā)送設(shè)備地址+寫信號(hào)ack_I2C();write_byte_I2C(address);/內(nèi)部寄存器地址ack_I2C();write_byte_I2C(date);/內(nèi)部寄存器數(shù)據(jù)ack_I2C();stop_I2C();uchar read_MUP6050_single(uchar address)uchar date;start_I2C();write_byte_I2C(SlaveAddress);/發(fā)送設(shè)備地址+寫信號(hào)ack_I2C();write_byte_I2C(address);/發(fā)送存儲(chǔ)單元地址，從開始ack_I2C();start_I2C();write_byte_I2C

25、(SlaveAddress+1);/發(fā)送設(shè)備地址+讀信號(hào)ack_I2C();date=read_byte_I2c();/讀出寄存器數(shù)據(jù)s_ack_I2C(1);/發(fā)送非應(yīng)答信號(hào)stop_I2C();return date;void init_MPU6050()/MUP6050初始化write_MUP6050_single(PWR_MGMT_1, 0x00);/解除休眠狀態(tài) write_MUP6050_single(SMPLRT_DIV, 0x07);/分頻產(chǎn)生采樣頻率采樣率=陀螺儀的輸出率/(1+SMPLRT_DIV) write_MUP6050_single(CONFIG, 0x06);/濾

26、波器參數(shù)設(shè)置 write_MUP6050_single(GYRO_CONFIG, 0x18);/陀螺儀XYZ軸不自校驗(yàn)陀螺儀輸出范圍/s write_MUP6050_single(ACCEL_CONFIG, 0x01);/加速度計(jì)XYZ軸不自校驗(yàn)Full Scale Range 2g 高通濾波器的截止設(shè)置int GETdate(uchar address)/獲取兩個(gè)字節(jié)的原始數(shù)據(jù)合并int H,L;int date;H=read_MUP6050_single(address);L=read_MUP6050_single(address+1);date=(H*256)+L);switch(add

27、ress) case GYRO_XOUT_H:date+=14;break;case GYRO_YOUT_H:date-=1;break;case GYRO_ZOUT_H:date+=11;break;return date;char get_A_angle()/針向上，利用三軸求出偏轉(zhuǎn)角度/double atan(double,double);即參數(shù)和返回值全為double類型char angle;double xx,yy,zz;xx=GETdate(ACCEL_XOUT_H);/獲取X軸上的數(shù)據(jù)yy=GETdate(ACCEL_YOUT_H);zz=GETdate(ACCEL_ZOUT_H

28、);/angle=atan(zz/sqrt(xx*xx+yy*yy)*(double)180/3.14;angle=atan2(yy,sqrt(xx*xx+zz*zz)*(double)180/3.14;/atan返回的是弧度值return angle;/double get_G_angle_speed(uchar GY)/返回角速度/int a;/a=GETdate(GY)/16.4;/角速度，度/秒/if(angle0)/angle=angle;/return a;/void get_G_angle()/用角速度積分/angle+=get_G_angle_speed(GYRO_ZOUT_H

29、)*0.0048;/把每次讀出的角速度累乘以時(shí)間累加起來/angle=get_A_angle()*0.1+angle*0.9;/void scan()/掃描按鍵if(key1=0)/模式一delayms(10);if(key1=0) mode=1; write_LCD_com(0x8a); write_LCD_data(mode+0x30);while(!key1);if(key2=0)/模式二delayms(10);if(key2=0)mode=2;write_LCD_com(0x8a);write_LCD_data(mode+0x30);while(!key2);if(key3=0)/模式三delayms(10);if(key3=0)mode=3;write_LCD_com(0x8a);write_LCD_data(mode+0x30);whil