電動(dòng)車蹺蹺板論文

1、2015年全國大學(xué)生電子設(shè)計(jì)競(jìng)賽電動(dòng)車蹺蹺板（F題）【本科組】2015年7月25日摘 要本方案以STM32F103RB單片機(jī)、直流電機(jī)驅(qū)動(dòng)電路、MPU-6050、反射式紅外傳感器、穩(wěn)壓模塊等電路構(gòu)成。小車采用前方的左中右三個(gè)反射式紅外傳感器，能沿著黑線在蹺蹺板上往返行駛，并始終保持在蹺蹺板上；同時(shí)，利用MPU-6050對(duì)小車當(dāng)前所在位置的傾斜角進(jìn)行測(cè)量。小車控制程序主要對(duì)采集信號(hào)分析轉(zhuǎn)換，結(jié)合PWM調(diào)速控制電機(jī)轉(zhuǎn)速和轉(zhuǎn)向，從而使小車快速在蹺蹺板上取得平衡；小車通過蜂鳴器來實(shí)現(xiàn)平衡指示以及實(shí)時(shí)顯示，從而完成整個(gè)設(shè)計(jì)過程。實(shí)驗(yàn)結(jié)果驗(yàn)證了該系統(tǒng)的性能滿足設(shè)計(jì)要求。關(guān)鍵詞：STM32F103RB MP

2、U-6050 反射式紅外傳感器 PWM調(diào)速 AbstractThe programs to STM32F103RB microcontroller, DC motor drive circuit, MPU-6050, reflective infrared sensor, voltage regulator modules and other circuits. Car used left, right in front of three reflective infrared sensor that can travel back and forth along the black line

3、 on a seesaw, and keep the seesaw; Meanwhile, MPU-6050 tilt angle of the trolley current location is measured. Car control program focuses on the acquisition signal analysis conversion, combined with PWM speed control motor speed and direction, allowing the car to quickly strike a balance on a seesa

4、w; trolley buzzer to achieve balance through instructions and real-time display, thus completing the entire design process. Experimental results demonstrate the performance to meet the design requirements of the system.Key word: STM32F103RB MPU-6050 PWM Speed Reflective infrared sensor目 錄1系統(tǒng)方案11.1 姿

5、態(tài)檢測(cè)模塊的論證與選擇11.2 電機(jī)驅(qū)動(dòng)模塊的論證與選擇11.3 穩(wěn)壓模塊的論證與選擇11.4 測(cè)量模塊的論證與選擇22系統(tǒng)理論分析與計(jì)算32.1 PID控制器設(shè)計(jì)32.2 基于卡爾曼濾波的數(shù)據(jù)融合33電路與程序設(shè)計(jì)43.1電路的設(shè)計(jì)43.1.1電路系統(tǒng)總體框圖43.1.2 電源系統(tǒng)電路43.1.3 電機(jī)驅(qū)動(dòng)模塊電路原理圖43.2程序的設(shè)計(jì)53.2.1程序流程圖54測(cè)試方案與測(cè)試結(jié)果84.1測(cè)試方案84.2 測(cè)試條件與儀器84.3 測(cè)試結(jié)果及分析84.3.1測(cè)試結(jié)果(數(shù)據(jù))84.3.2測(cè)試分析與結(jié)論95總結(jié)9附錄101.電路原理圖10 2.元器件清單12 3.主要源程序131系統(tǒng)方案本系統(tǒng)主要

6、由姿態(tài)檢測(cè)模塊、電機(jī)驅(qū)動(dòng)模塊、測(cè)量模塊、穩(wěn)壓模塊、電源模塊組成。下面分別論證各個(gè)模塊的選擇。1.1 姿態(tài)檢測(cè)模塊的論證與選擇方案一：ENC-03。這是一款利用科里奧利力原理輸出一個(gè)與角速度成正比的模擬電壓信號(hào)的角速度傳感器。方案二：MPU-6050。MPU-60X0 是全球首例9軸運(yùn)動(dòng)處理傳感器。它集成了3軸MEMS陀螺儀，3軸MEMS加速度計(jì)，以及一個(gè)可擴(kuò)展的數(shù)字運(yùn)動(dòng)處理器DMP（Digital Motion Processor），可用I2C接口連接一個(gè)第三方的數(shù)字傳感器，比如磁力計(jì)。擴(kuò)展之后就可以通過其I2C或SPI接口輸出一個(gè)9軸的信號(hào)（SPI 接口僅在MPU-6000可用）。MPU-6

7、0X0也可以通過其I2C接口連接非慣性的數(shù)字傳感器，比如壓力傳感器。方案一所用模塊功能較單一，只是單獨(dú)的陀螺儀。方案二所用模塊功能較多，不僅有陀螺儀功能，且有加速度計(jì)的功能。且方案一所用模塊較為精密，容易損壞，故選用方案二。1.2 電機(jī)驅(qū)動(dòng)模塊的論證與選擇方案一：TB6612FNG。TB6612FNG是一款新型直流電機(jī)驅(qū)動(dòng)器，它具有集成度高、驅(qū)動(dòng)能力強(qiáng)以及控制方式靈活等特點(diǎn)。方案二：L298N。具有信號(hào)指示，轉(zhuǎn)數(shù)可調(diào)，抗干擾能力強(qiáng)，具有過電壓和過電流保護(hù)，可單獨(dú)控制兩臺(tái)直流電機(jī)，可單獨(dú)控制一臺(tái)步進(jìn)電機(jī)。方案三：ULN2003，ULN2003 是高耐壓、大電流復(fù)合晶體管陣列，由七個(gè)硅NPN 復(fù)合

8、晶體管組成。最高接5V電壓。方案一所用電機(jī)體積小，重量小。本實(shí)驗(yàn)要求小車自重不能過輕。切方案一所用電機(jī)體內(nèi)電子元件較為精密，易損壞，故不用方案一。方案三所用電機(jī)電壓范圍太小，本實(shí)驗(yàn)要求電壓要穩(wěn)定在較高值，故不選用方案三。所以選擇方案二。1.3 穩(wěn)壓模塊的論證與選擇方案一：LM2596。LM2596系列是德州儀器（TI）生產(chǎn)的3A電流輸出降壓開關(guān)型集成穩(wěn)壓芯片，它內(nèi)含固定頻率振蕩器（150KHZ）和基準(zhǔn)穩(wěn)壓器（1.23v），并具有完善的保護(hù)電路、電流限制、熱關(guān)斷電路等。利用該器件只需極少的外圍器件便可構(gòu)成高效穩(wěn)壓電路。方案二：LM2576系列是美國國家半導(dǎo)體公司生產(chǎn)的3A電流輸出降壓開關(guān)型集成穩(wěn)

9、壓電路，它內(nèi)含固定頻率振蕩器(52kHz)和基準(zhǔn)穩(wěn)壓器(1.23V)，并具 有完善的保護(hù)電路，包括電流限制及熱關(guān)斷電路等，利用該器件只需極少的外圍器件便可構(gòu)成高效穩(wěn)壓電路。方案一所用穩(wěn)壓模塊開關(guān)頻率高，效率更高。方案二所用穩(wěn)壓模塊太過昂貴，性價(jià)比比方案一所用穩(wěn)壓模塊低。所以選用方案一。1.4 測(cè)量模塊的論證與選擇方案一：用光敏電阻組成光敏探測(cè)器。光敏電阻的阻值可以跟隨周圍環(huán)境光線的變化而變化。當(dāng)光線照射到蹺蹺板上面時(shí)，光線發(fā)射強(qiáng)烈，光線照射到黑線上面時(shí)，光線發(fā)射較弱。因此光敏電阻在蹺蹺板和黑線上方時(shí)，阻值會(huì)發(fā)生明顯的變化。將阻值的變化值經(jīng)過比較器就可以輸出高低電平。但是這種方案受光照影響很大，

10、并且不能夠穩(wěn)定的工作。因此我們考慮其他更加穩(wěn)定的方案。方案二：用紅外光電對(duì)管尋跡傳感器?，F(xiàn)有的封裝好的紅外對(duì)管應(yīng)用電路簡(jiǎn)單，工作穩(wěn)定，再加上控制芯片的電壓比器功能模塊處理采集信號(hào)，容易實(shí)現(xiàn)題目要求。方案一受光照影響很大，并且不能夠穩(wěn)定的工作，本實(shí)驗(yàn)要求測(cè)量模塊能穩(wěn)定工作，故不選用方案一。方案二容易實(shí)現(xiàn)題目要求。因此本系統(tǒng)選擇方案二。在平衡系統(tǒng)中，根據(jù)要求，只要蹺蹺板兩端與地面的距離差小于40mm即可認(rèn)為平衡，本設(shè)計(jì)通過傾角傳感器檢測(cè)蹺蹺板水平傾角，所以只要水平傾角保持在0°附近的某個(gè)角度范圍之內(nèi)即認(rèn)為蹺蹺板達(dá)到平衡狀態(tài)。其閉環(huán)結(jié)構(gòu)框圖如圖所示。該系統(tǒng)的工作原理是:小車駛上蹺蹺板后，通

11、過傾角傳感器不斷測(cè)量蹺蹺板的傾角(即實(shí)際傾角)，該實(shí)際傾角與給定傾角作比較，形成傾角偏差，通過直流電機(jī)控制小車前后微移，不斷修正該傾角偏差，最終使傾角保持在給定范圍之內(nèi)，此時(shí)蹺蹺板便達(dá)到平衡狀態(tài)。2系統(tǒng)理論分析與計(jì)算2.1 PID控制器設(shè)計(jì)PID控制器由比例單元（P）、積分單元（I）和微分單元（D）組成。其輸入e (t）與輸出u (t）的關(guān)系為： 其中為比例系數(shù)；為積分時(shí)間常數(shù)；為微分時(shí)間常數(shù)。PID控制器具有原理簡(jiǎn)單、使用方便、適應(yīng)性強(qiáng)、魯棒性強(qiáng)、對(duì)模型依賴少等特點(diǎn)，因此使用PID控制器實(shí)現(xiàn)兩輪自平衡車的控制是完全可行的。2.2 基于卡爾曼濾波的數(shù)據(jù)融合 卡爾曼濾波器解決離散時(shí)間控制過程的一

12、般方法，首先定義模型線性隨機(jī)微分方程。假設(shè)卡爾曼濾波模型k時(shí)刻真實(shí)狀態(tài)是從（k-1）時(shí)刻推算出來，如下式 式中，是k時(shí)刻狀態(tài)；A是k-1時(shí)刻狀態(tài)變換模型；B是作用在控制器向量上的輸入控制模型；是過程噪聲，假設(shè)其均值為零，協(xié)方差矩陣符合多元正態(tài)分布： k時(shí)刻對(duì)應(yīng)真實(shí)狀態(tài)的測(cè)量滿足下式： 式2-17中是觀測(cè)模型，將真實(shí)控制映射為觀測(cè)空間；為觀測(cè)噪聲，其均值為零，協(xié)方差矩陣符合正態(tài)分布：初始狀態(tài)以及每一時(shí)刻的噪聲都認(rèn)為是互相獨(dú)立的?？柭鼮V波器的操作主要包括兩個(gè)階段：預(yù)估與更新。在預(yù)估階段，濾波器根據(jù)上一時(shí)刻狀態(tài)，估算出當(dāng)前時(shí)刻狀態(tài)；在更新階段，濾波器利用當(dāng)前時(shí)刻觀測(cè)值優(yōu)化在預(yù)估階段獲得的測(cè)量值，以

13、獲得一個(gè)更準(zhǔn)確的新估計(jì)值111213。3電路與程序設(shè)計(jì)3.1電路的設(shè)計(jì)3.1.1電路系統(tǒng)總體框圖3.1.2 電源系統(tǒng)電路3.1.3 電機(jī)驅(qū)動(dòng)模塊電路原理圖3.2程序的設(shè)計(jì)3.2.1程序流程圖1、主程序流程圖 當(dāng)小車復(fù)位時(shí)，開始運(yùn)行主程序流程。開始 - 調(diào)用平衡子程序 - 進(jìn)入判斷 - 是（否） -調(diào)用平衡指示子程序（回到上一步） - 延時(shí) - 電機(jī)運(yùn)行向前 - 調(diào)用測(cè)量子程序 - 進(jìn)入判斷 - 是（否） - 延時(shí)（回到上兩步） - 電機(jī)運(yùn)行向后 - 結(jié)束2、平衡子程序流程圖開始 - 平衡測(cè)量模塊檢測(cè) - 進(jìn)入判斷 - 是（否） - 調(diào)用電機(jī)驅(qū)動(dòng) - 電機(jī)正轉(zhuǎn)（電機(jī)反轉(zhuǎn)） - 進(jìn)入判斷 - 是（

14、否） - 結(jié)束（回到第一步）3、平衡指示子程序流程圖開始 - 平衡測(cè)量模塊檢測(cè) - 進(jìn)入判斷 - 是（否） - 調(diào)用計(jì)數(shù)器（回到上一步） - 進(jìn)入判斷 - 是（否) - 調(diào)用蜂鳴器（回到第一步） - 結(jié)束4、測(cè)量子程序流程圖開始 - 三組光線測(cè)量模塊檢測(cè) - 進(jìn)入判斷 - 是（否） - 調(diào)用電機(jī)驅(qū)動(dòng)（回到上一步） - 電機(jī)運(yùn)行通過控制轉(zhuǎn)速調(diào)整方向 - 進(jìn)入判斷 - 是（否） - 結(jié)束（回到第一步）4測(cè)試方案與測(cè)試結(jié)果4.1測(cè)試方案1.硬件測(cè)試 檢測(cè)蹺蹺板的尺寸； 檢測(cè)蹺蹺板的可用性； 檢測(cè)小車線路連接狀態(tài)；2. 運(yùn)行測(cè)試 檢測(cè)小車在蹺蹺板上面的運(yùn)行情況； 檢測(cè)小車對(duì)題目各項(xiàng)要求的完成度。4.2

15、 測(cè)試條件與儀器測(cè)試條件：檢查多次，硬件電路必須與系統(tǒng)原理圖完全相同，并且檢查無誤，硬件電路保證無虛接。 小車要能完成題目要求。測(cè)試儀器： 測(cè)試蹺蹺板：卷尺。 測(cè)試小車：蹺蹺板（需檢測(cè)合格），秒表。4.3 測(cè)試結(jié)果及分析4.3.1測(cè)試結(jié)果(數(shù)據(jù))硬件：檢查多次，硬件電路與系統(tǒng)原理圖完全相同，硬件電路無虛接以下為硬件檢測(cè)數(shù)據(jù)：蹺蹺板總體長(zhǎng)度：1600mm；蹺蹺板總體寬度：300mm；蹺蹺板中心軸部分高度：83mm；小車整體寬度：200mm；小車整體長(zhǎng)度：300mm；運(yùn)行：不加配重情況下表1：從A點(diǎn)到C點(diǎn)的時(shí)間測(cè)試測(cè)試項(xiàng)目第1次第2次第3次第4次AC所用時(shí)間/s1512129表2：平衡點(diǎn)測(cè)試測(cè)試項(xiàng)

16、目第1次第2次第3次第4次尋找平衡點(diǎn)時(shí)間/s30402420d=dA-dB/mm30.520.522.814.7表3：平衡點(diǎn)到B的時(shí)間測(cè)試測(cè)試項(xiàng)目第1次第2次第3次第4次平衡點(diǎn)B所用時(shí)間/s4323車頭到B點(diǎn)的距離/mm35203431表4：平衡點(diǎn)倒退回A點(diǎn)的時(shí)間測(cè)試測(cè)試項(xiàng)目第1次第2次第3次第4次B點(diǎn)停止時(shí)間/s5555BA所用時(shí)間/s2326257根據(jù)測(cè)量，完成全過程的總時(shí)間均小于180秒。4.3.2測(cè)試分析與結(jié)論根據(jù)上述測(cè)試數(shù)據(jù)，小車及蹺蹺板尺寸合格，小車達(dá)成目標(biāo)時(shí)間均小于題目要求時(shí)間，由此可以得出以下結(jié)論：1、小車及蹺蹺板硬件條件符合題目要求。2、小車內(nèi)部程序無問題，小車能夠正常運(yùn)行。

17、3、小車能完成題目各項(xiàng)要求，且有自主發(fā)揮空間。綜上所述，本設(shè)計(jì)達(dá)到設(shè)計(jì)要求。五、總結(jié)在本次課題訓(xùn)練中，我們基本完成了題目各項(xiàng)要求。從本次設(shè)計(jì)中我們體會(huì)到，對(duì)小車實(shí)施控制不僅是電子控制問題，其中也涉及到了力學(xué)和光學(xué)等方面的知識(shí)。在有限的時(shí)間里未能完美的解決小車尋找平衡點(diǎn)的的問題。在對(duì)直流電機(jī)的調(diào)速和精確控制上還不是很靈活，在以后的訓(xùn)練中應(yīng)該多加學(xué)習(xí)和鍛煉。22附錄1.電路原理圖單片機(jī)最小系統(tǒng)及時(shí)鐘電路原理圖MPU6050電路圖復(fù)位及按鍵接口電路2.元器件清單 電 動(dòng) 小 車序號(hào)名稱、型號(hào)、規(guī)格數(shù)量1STM32F103RB單片機(jī)12直流電機(jī)43MPU-605014電機(jī)驅(qū)動(dòng)L298N25紅外傳感器3

18、6穩(wěn)壓模塊LM259627電源（航模電池）18小車框架（高160mm，長(zhǎng)300mm，寬200mm）110輔助連接板1 蹺 蹺 板序號(hào)名稱、型號(hào)、規(guī)格數(shù)量11600mm * 300mm * 14mm木板12135mm * 25mm * 45mm木支架23轉(zhuǎn)軸（50mm螺釘）2465mm * 25mm * 45mm 轉(zhuǎn)軸固定架25310mm * 25mm * 45mm木軸承16黑膠布若干7引導(dǎo)裝置（硬紙板，硬木板）18連接用螺釘若干9配重若干3.源程序/* Includes -*/#include "stm32f10x.h"#include "delay.h"

19、;#include "sys.h"#include "inv_mpu_dmp_motion_driver.h"#include "inv_mpu.h"#include "math.h"#include "usart.h"#include "STM32_I2C.h"#include <stdio.h>#include <pwm.h>#include <pid.h>#include <tim.h> #define PRINT_ACC

20、EL (0x01)#define PRINT_GYRO (0x02)#define PRINT_QUAT (0x04)#define ACCEL_ON (0x01)#define GYRO_ON (0x02)#define MOTION (0)#define NO_MOTION (1)/* Starting sampling rate. */#define DEFAULT_MPU_HZ (100)#define FLASH_SIZE (512)#define FLASH_MEM_START (void*)0x1800)uint32_t TimingDelay = 0;extern int k;

21、extern volatile u32 time; uint16_t a1,a2,b1,b2;extern float Kp;extern float Ki;extern float Kd;int s1=0;int s2=0;int s3=0;int s4=0;int num=0;int qq=0;int xj=0;extern float output;extern float SetSpeed;uint8_t USART_RXBUF10;extern uint8_t RXOVER;void Delay(_IO uint32_t nCount);void Clock_Enable(void)

22、;void GPIO_Configuration(void);void check(void);void turn(void);void bee(void);void Delay_Ms(uint32_t nTime) TimingDelay = nTime; while(TimingDelay != 0);struct rx_s unsigned char header3; unsigned char cmd;struct hal_s unsigned char sensors; unsigned char dmp_on; unsigned char wait_for_tap; volatil

23、e unsigned char new_gyro; unsigned short report; unsigned short dmp_features; unsigned char motion_int_mode; struct rx_s rx;static struct hal_s hal = 0;volatile unsigned char rx_new;float Roll,Yaw;float Pitch;static signed char gyro_orientation9 = -1, 0, 0, 0,-1, 0, 0, 0, 1;enum packet_type_e PACKET

24、_TYPE_ACCEL,PACKET_TYPE_GYRO,PACKET_TYPE_QUAT,PACKET_TYPE_TAP,PACKET_TYPE_ANDROID_ORIENT,PACKET_TYPE_PEDO,PACKET_TYPE_MISC ;static unsigned short inv_row_2_scale(const signed char *row) unsigned short b; if (row0 > 0) b = 0; else if (row0 < 0) b = 4; else if (row1 > 0) b = 1; else if (row1

25、< 0) b = 5; else if (row2 > 0) b = 2; else if (row2 < 0) b = 6; else b = 7; / error return b;static unsigned short inv_orientation_matrix_to_scalar( const signed char *mtx) unsigned short scalar; scalar = inv_row_2_scale(mtx); scalar |= inv_row_2_scale(mtx + 3) << 3; scalar |= inv_row

26、_2_scale(mtx + 6) << 6; return scalar;static void run_self_test(void) int result;/ char test_packet4 = 0; long gyro3, accel3; result = mpu_run_self_test(gyro, accel); if (result = 0x7) /* Test passed. We can trust the gyro data here, so let's push it down * to the DMP. */ float sens; unsig

27、ned short accel_sens; mpu_get_gyro_sens(&sens); gyro0 = (long)(gyro0 * sens); gyro1 = (long)(gyro1 * sens); gyro2 = (long)(gyro2 * sens); dmp_set_gyro_bias(gyro); mpu_get_accel_sens(&accel_sens); accel0 *= accel_sens; accel1 *= accel_sens; accel2 *= accel_sens; dmp_set_accel_bias(accel);prin

28、tf("setting bias succesfully .n"); elseprintf("bias has not been modified .n");#define q30 1073741824.0ffloat q0=1.0f,q1=0.0f,q2=0.0f,q3=0.0f;void Read_MPU_Data(void)unsigned long sensor_timestamp; short gyro3, accel3, sensors; unsigned char more; long quat4; /float Yaw,Roll,Pitc

29、h; dmp_read_fifo(gyro, accel, quat, &sensor_timestamp, &sensors, &more); if (sensors & INV_WXYZ_QUAT ) q0=quat0 / q30; q1=quat1 / q30; q2=quat2 / q30; q3=quat3 / q30; Pitch = asin(-2 * q1 * q3 + 2 * q0* q2)* 57.3; Roll = atan2(2 * q2 * q3 + 2 * q0 * q1, -2 * q1 * q1 - 2 * q2* q2 + 1)

30、* 57.3; / roll Yaw = atan2(2*(q1*q2 + q0*q3),q0*q0+q1*q1-q2*q2-q3*q3) * 57.3; if(sensors & INV_XYZ_GYRO) if(sensors & INV_XYZ_ACCEL) void gpio_config(void) GPIO_InitTypeDef GPIO_InitStructure;RCC_APB2PeriphClockCmd(RCC_APB2Periph_GPIOC, ENABLE);GPIO_InitStructure.GPIO_Pin = GPIO_Pin_0|GPIO_P

31、in_1|GPIO_Pin_2;GPIO_InitStructure.GPIO_Mode = GPIO_Mode_IN_FLOATING;GPIO_Init(GPIOC, &GPIO_InitStructure);void BEE(void)GPIO_InitTypeDef GPIO_InitStructure;RCC_APB2PeriphClockCmd(RCC_APB2Periph_GPIOB, ENABLE);RCC_APB2PeriphClockCmd(RCC_APB2Periph_AFIO,ENABLE);GPIO_PinRemapConfig(GPIO_Remap_SWJ_

32、NoJTRST,ENABLE); GPIO_PinRemapConfig(GPIO_Remap_SWJ_Disable,ENABLE);GPIO_PinRemapConfig(GPIO_Remap_SWJ_JTAGDisable,ENABLE);GPIO_InitStructure.GPIO_Pin = GPIO_Pin_4; GPIO_InitStructure.GPIO_Speed = GPIO_Speed_50MHz; GPIO_InitStructure.GPIO_Mode = GPIO_Mode_Out_PP; GPIO_Init(GPIOB, &GPIO_InitStruc

33、ture);void be(void)int i;GPIO_ResetBits(GPIOB,GPIO_Pin_4); for(i=0x010000; i>0; i-);GPIO_SetBits(GPIOB,GPIO_Pin_4); void turn(void)if(GPIO_ReadInputDataBit(GPIOC,GPIO_Pin_0)=1) TIM_Config1(0,0);/左轉(zhuǎn) TIM_Config2(0,800);else if(GPIO_ReadInputDataBit(GPIOC,GPIO_Pin_1)=1) TIM_Config1(0,800);/右轉(zhuǎn) TIM_Co

34、nfig2(0,0);void check(void)if(GPIO_ReadInputDataBit(GPIOC,GPIO_Pin_2)=1) while(GPIO_ReadInputDataBit(GPIOC,GPIO_Pin_2)=1); Delay(4000000);num=num+1; if(num=1)/踩黑線平衡 SetSpeed=1.8; else if(num=2)SetSpeed=8;else if(num=3)/踩黑線后退SetSpeed=-8;else if(num=5)while(1) TIM_Config1(0,0);TIM_Config2(0,0); int ma

35、in(void)int result,i; Clock_Enable(); GPIO_Configuration(); USART_Config(); i2cInit();PWM_IO_Config();TIM4_NVIC_Configuration(); TIM4_Configuration();BEE();gpio_config(); RCC_APB1PeriphClockCmd(RCC_APB1Periph_TIM4 , ENABLE); result = mpu_init(); if(!result) /mpu_init(); printf("mpu initializati

36、on complete.n "); /mpu_set_sensor if(!mpu_set_sensors(INV_XYZ_GYRO | INV_XYZ_ACCEL) printf("mpu_set_sensor complete .n"); else printf("mpu_set_sensor come across error .n"); /mpu_configure_fifo if(!mpu_configure_fifo(INV_XYZ_GYRO | INV_XYZ_ACCEL) printf("mpu_configure_f

37、ifo complete .n"); else printf("mpu_configure_fifo come across error .n"); /mpu_set_sample_rate if(!mpu_set_sample_rate(DEFAULT_MPU_HZ) printf("mpu_set_sample_rate complete .n"); else printf("mpu_set_sample_rate error .n"); /dmp_load_motion_driver_firmvare if(!dmp_

38、load_motion_driver_firmware() printf("dmp_load_motion_driver_firmware complete .n"); else printf("dmp_load_motion_driver_firmware come across error .n"); /dmp_set_orientation if(!dmp_set_orientation(inv_orientation_matrix_to_scalar(gyro_orientation) printf("dmp_set_orientati

39、on complete .n"); else printf("dmp_set_orientation come across error .n"); /dmp_enable_feature if(!dmp_enable_feature(DMP_FEATURE_6X_LP_QUAT | DMP_FEATURE_TAP | DMP_FEATURE_ANDROID_ORIENT | DMP_FEATURE_SEND_RAW_ACCEL | DMP_FEATURE_SEND_CAL_GYRO | DMP_FEATURE_GYRO_CAL) printf("dmp

40、_enable_feature complete .n"); else printf("dmp_enable_feature come across error .n"); /dmp_set_fifo_rate if(!dmp_set_fifo_rate(DEFAULT_MPU_HZ) printf("dmp_set_fifo_rate complete .n"); else printf("dmp_set_fifo_rate come across error .n"); run_self_test(); if(!mpu_

41、set_dmp_state(1) printf("mpu_set_dmp_state complete .n"); else printf("mpu_set_dmp_state come across error .n"); GPIO_ResetBits(GPIOA, GPIO_Pin_0 | GPIO_Pin_1| GPIO_Pin_6| GPIO_Pin_7);GPIO_ResetBits(GPIOC,GPIO_Pin_0|GPIO_Pin_1|GPIO_Pin_2); while(1) if(RXOVER = 1)for(i=0;i<20;i+)USART_RXBUFi = 0; /清空接收區(qū)RXOVER = 0;USART_ITConfig(USART2,USART_IT_RXNE,ENABLE);turn();check();if(k=1)k=0;if(time=1)Read_MPU_Data();if(time>=5)time=0;PID_realize(); if(ou