基于單片機的平衡車系統(tǒng)硬件和軟件設(shè)計案例1系統(tǒng)硬件電路設(shè)計根據(jù)對系統(tǒng)的方案設(shè)計及各個模塊的選型結(jié)果，系統(tǒng)的硬件部分主要包括：單片機最小系統(tǒng)電路、電源電路、姿態(tài)傳感器模塊、電機驅(qū)動模塊、超聲波避障模塊、顯示模塊等。1.1單片機最小系統(tǒng)單片機最小系統(tǒng)主要由電源電路、復(fù)位電路、時鐘電路以及擴展部分等部分組成。STM32具有三種復(fù)位方式：電源復(fù)位、系統(tǒng)復(fù)位、看門狗復(fù)位。當(dāng)電源復(fù)位時，NRST引腳被拉低，產(chǎn)生外部復(fù)位脈沖使系統(tǒng)復(fù)位。其中的電容C52的目的是按鍵消抖，防止在按鍵剛剛接觸或松開時的電平抖動引發(fā)誤動作，按鍵閉合或松開的接觸過程大約有10ms的抖動，這對于主控芯片I/O控制來說已經(jīng)是很長的時間，足以執(zhí)行多次復(fù)位動作。由于電容電壓不會突變，所以采用電容濾波，防止抖動復(fù)位誤動作[14][15]。圖1.1復(fù)位電路去耦電容主要用來濾除雜波，保持引腳電壓的穩(wěn)定。圖1.2去耦電容根據(jù)芯片數(shù)據(jù)手冊，晶振振蕩頻率4-16M可選。晶體振蕩器是由石英制成的，晶體振蕩器之所以能作為振蕩器使用，是由于在晶片的兩極施加電場時晶體發(fā)生機械變形的壓電效應(yīng)。當(dāng)在晶體晶片上施加交流電壓時，晶體引起機械振動，機械變形振動產(chǎn)生交變電場，這種交變電場的電壓很微弱，但振動頻率很穩(wěn)定。當(dāng)外加交流電壓的頻率與晶片的固有頻率相同時，機械振動幅度突然增大的現(xiàn)象稱為“壓電諧振”。晶振為主控芯片提供系統(tǒng)時鐘，所有外設(shè)運行，CPU在這個時鐘上運行。這類似于整個系統(tǒng)的“心跳”。晶振可分為無源振蕩器和有源振蕩器，基本都是穿通振蕩器，但在微控制器的情況下，微控制器集成了一個帶有電阻和電源的反相放大器，外接晶振和電容就足夠了，稱為無源晶體振蕩器。有源晶振是整體穿通振蕩器，直接加電工作，價格比無源晶振貴[16]。圖1.3振蕩電路最小系統(tǒng)原理圖如圖所示。圖1.4最小系統(tǒng)1.2電源電路對于全電子化設(shè)計，主要問題是提供貫穿整個系統(tǒng)的電源模塊，電源模塊的穩(wěn)定性和可靠性是系統(tǒng)順利運行的前提和基礎(chǔ)。平衡小車動力源為2S大電流航模鋰電池，每節(jié)電池額定電壓1.7v（充滿電壓2.1v），兩節(jié)電池串聯(lián)得到的7.4v（充滿電壓8.2v）。但是，電路中常用的電壓為5v和1.3v，系統(tǒng)的部分芯片需要5V供電，部分芯片需要1.3V供電。所以，我們并不能直接把電池電壓供給電路模塊，而是需要對7.4v進一步降壓穩(wěn)壓，分別降壓至5v和1.3v，以供整個電路系統(tǒng)使用。穩(wěn)壓芯片選用LM2940-5.0、LM1117-1.3兩款低壓差線性穩(wěn)壓芯片（LDO），兩者的最大輸出電流分別為1000mA、800mA，不俗的輸出電流能夠滿足小車的正常工作，并且還能給后續(xù)的擴展預(yù)留了電流余量。另外，LM2940-5.0、LM1117-1.3兩款LDO芯片工作效率高、輸出電壓紋波比DC/DC芯片要小得多，適合單片機供電。整個系統(tǒng)需要3種電源：7.2V電源，為驅(qū)動電機供電。5V電源，為單片機及相關(guān)外設(shè)供電。1.3V電源，為陀螺儀及加速度計供電。5V電源由LM2940提供，LM2940是一款具有低輸出紋波的線性低壓差3端穩(wěn)壓器。LM117用作1.3V電源。為了給傳感器MPU-6050提供穩(wěn)定干凈的電源，XC6206特意只給MPU-6050供電。穩(wěn)壓模塊如圖1.5所示。圖1.5穩(wěn)壓模塊1.3六軸傳感器電路陀螺儀、加速度計都是慣性測量元件的一種。而MPU-6050傳感器的內(nèi)部同時集成了陀螺儀和加速度傳感器兩種慣性測量元件[18]。MPU-6050是InvenSense推出的全球首款集成6軸運動處理模塊，相比多組件解決方案，MPU-6050消除和安裝陀螺儀和加速器組合時的錯位問題，有效減少空間。MPU-6050集成了3軸陀螺儀和3軸加速度計，包括可用于連接外部磁傳感器的IIC從接口，并使用嵌入式數(shù)字運動處理器硬件加速引擎。9軸融合計算的完整數(shù)據(jù)最終從IIC主界面輸出到應(yīng)用程序。DMP可以使用InvenSense提供的運動處理數(shù)據(jù)庫輕松實現(xiàn)姿勢計算，降低操作系統(tǒng)的運動處理操作難度并減少開發(fā)挑戰(zhàn)。圖1.6MPU-6050的內(nèi)部框圖其中SCL和SDA接MCU的I2C主接口，MCU通過這個IIC接口控制MPU-6050，另外還有一個IIC從接口：AUX_CL和AUX_DA，這個接口連接外部從設(shè)備可以使用用于可組成9軸傳感器的磁傳感器等設(shè)備。VLOGIC是IO口上的電壓，這個引腳可以降低到1.8v。您通?？梢灾苯舆B接到VDD。AD0是從IIC接口（連接到MCU）的地址控制引腳，它控制IIC地址的最低有效位。MPU6050的IIC地址接GND：0X68，接VDD時為0X69MPU-6050模塊中AD0默認接GND，所以此時的IIC地址為MPU-6050為0X68（不包括最低有效位）。圖1.7顯示了MPU6050的傳感器原理圖。圖1.7MPU6050電路圖1.5電機驅(qū)動電路根據(jù)系統(tǒng)設(shè)計需求，由于小車需要兩組電機，因此也需要兩路H橋驅(qū)動電路。而我們使用的TTM1直流減速電機扭矩和速度都適中，堵轉(zhuǎn)電流最大不超過3A。于是，我們選用電機驅(qū)動芯片TB6612FNG組成了電機驅(qū)動電路，一塊TB6612FNG驅(qū)動芯片內(nèi)置了兩組H橋電路。TB6612FNG最大輸出電流可達1.2A，因此不必擔(dān)心因堵轉(zhuǎn)或調(diào)試不當(dāng)引起的燒電機或燒驅(qū)動。脈寬調(diào)制方式產(chǎn)生改變占空比的PWM信號，通過快速切換驅(qū)動器的輸出狀態(tài)來實現(xiàn)對電機的速度控制。PWM占空比的大小決定了平均輸出電壓，它決定了電機的速度。表1.1顯示了TB6612FNG的邏輯真值表。如果設(shè)備正在工作，則STBY引腳設(shè)置為高電平。IN1和IN2不變。可以通過調(diào)整PWM引腳的輸入信號來控制電機的單向速度。PWM引腳設(shè)置為高電平，IN1和IN2輸入被調(diào)整。該信號可用于控制電機的雙向速度。表中通道A和B的控制邏輯相同。電機驅(qū)動模塊電路圖如圖1.8所示。表1.1TB6612FNG邏輯真值表輸入輸出IN1IN2PWMSTBY0102模式狀態(tài)HHH/LHLL制動LHHHLH反轉(zhuǎn)LHLHLL制動HLHHHL正轉(zhuǎn)HLLHLL制動LLHHOFF停止H/LH/LH/LLOFF待機圖1.8TB6612FNG模塊電路圖1.6TTM1直流減速電機直流電動機的基本構(gòu)造包括電樞、場磁鐵、集電環(huán)、電刷。電樞是軟鐵芯，可以繞多個線圈纏繞并繞軸旋轉(zhuǎn)，而場磁體是產(chǎn)生磁場的強大的永磁體或電磁體。集電環(huán)是具有兩個半圓形線圈的集電環(huán)，大約連接到線圈的兩端，可用于在線圈旋轉(zhuǎn)時直流電。流過線圈的電流方向每轉(zhuǎn)半圈就會改變一次。電刷通常由碳制成，集電環(huán)在固定位置接觸電刷以連接電源。1.6.1直流電機原理如圖1.9所示是一個簡單的直流電機。當(dāng)線圈通電時，轉(zhuǎn)子周圍會產(chǎn)生磁場，轉(zhuǎn)子的左側(cè)與左側(cè)的磁鐵分離并被吸引到右側(cè)，從而引起旋轉(zhuǎn)。轉(zhuǎn)子由于慣性而不斷旋轉(zhuǎn)，當(dāng)轉(zhuǎn)子移動到水平位置時，電流轉(zhuǎn)換器反轉(zhuǎn)線圈的電流方向，同時反轉(zhuǎn)線圈產(chǎn)生的磁場，所以這個過程可重復(fù)。圖1.9直流電機工作原理減速機又稱齒輪箱，常用于低速、大扭矩的傳動裝置，將高速旋轉(zhuǎn)（每分鐘幾千轉(zhuǎn)）的原動機的動力通過齒輪傳遞到輸出減速機輸入軸采用齒數(shù)較少的大齒輪，以達到減速的目的。傳動比是大齒輪和小齒輪的齒數(shù)之比。減速機是一種比較精密的機械，它匹配執(zhí)行器之間的速度并傳遞扭矩，用于減速和增加扭矩的目的。減速機的種類很多，按轉(zhuǎn)動和結(jié)構(gòu)特點分，減速機的種類有齒輪減速機、蝸輪蝸桿減速機、蝸輪減速機和齒輪蝸輪蝸桿減速機、行星齒輪減速機、擺線減速機、諧波齒輪減速機等等。齒輪減速器體積小，傳遞扭矩大，但有一定的回程間隙。如圖1.10所示。圖1.10齒輪減速器蝸桿減速機具有反向自鎖功能，可以提高減速比，但如圖1.11所示，一般尺寸較大，傳動效率低，精度差。圖1.11蝸桿減速器行星減速機的主要特點是結(jié)構(gòu)緊湊、回程間隙小、精度高、壽命長、額定輸出扭矩大，如圖1.12所示，但價格昂貴。圖1.12行星減速器本次設(shè)計選用的TTM1直流減速電機是搭配齒輪減速器的電機，如圖1.13所示。圖1.13TTM1直流減速電機1.6.2編碼器介紹編碼器是一種旋轉(zhuǎn)傳感器，可將角位移或角速度轉(zhuǎn)換為一系列電子數(shù)字脈沖?？梢酝ㄟ^編碼器測量較低的位移或速度信息。編碼器類型根據(jù)輸出數(shù)據(jù)的類型分為增量式編碼器和絕對式編碼器。根據(jù)編碼器的檢測原理，可分為光學(xué)式、磁式、電感式和電容式。常見的有光電編碼器（光學(xué)）和霍爾編碼器（磁性）。光電編碼器的采集精度遠高于霍爾編碼器，但價格較貴[18]。編碼器行數(shù)是轉(zhuǎn)一圈后A(B)輸出的脈沖數(shù)，下圖中的AB是輸出脈沖信號線，一圈后脈沖數(shù)相同，但是相位差為90°。線數(shù)越高，編碼器所能反映的位置精度越高。相位差是90°。通過判斷哪個信號在前，哪個信號在后，就可以知道電機是在向前旋轉(zhuǎn)還是向后旋。脈沖信號線如圖1.14所示。圖1.14脈沖信號線本設(shè)計使用的電機為TTM1直流減速電機，它的尾部加裝了霍爾編碼器，直接輸出AB相脈沖，電機接口如圖1.15所示。圖1.15電機接口1.7超聲波避障模塊HC-SR04超聲波遙測模塊可提供約2-400cm的非接觸式距離檢測功能，遙測精度可達3mm。該模塊包括超聲波發(fā)射器、接收器和控制電路，就像智能汽車和轉(zhuǎn)向，或者在某些項目中經(jīng)常使用。智能小車遙測可以提前檢測到障礙物，使智能小車及時轉(zhuǎn)彎避開障礙物。實物圖如圖1.16所示，電路圖如圖1.17所示。HC-SR04是單片機應(yīng)用中常用的超聲波測距模塊。超聲波發(fā)射器向一定方向發(fā)射超聲波，并在傳輸過程中開始計時。超聲波在空氣中傳播并在途中遇到障礙物時立即返回。超聲波接收器接收到反射波后立即停止計時。聲波在空氣中的傳播速度為340m/s，根據(jù)計時器記錄的時間t，就可以計算出發(fā)射點距障礙物的距離s，即：s=340m/s?t/2。這就是時間差測距法。HC-SR04超聲波測距模塊測量距離：2cm~450cm。圖1.16HC-SR04超聲波測距模塊圖1.17超聲波測距模塊電路圖1.8OLED顯示OLED，即有機發(fā)光二極管（OrganicLight-EmittingDiode），又稱為有機電激光顯示（OrganicElectroluminesenceDisplay，OELD）。OLED由于同時具備自發(fā)光，不需背光源、對比度高、厚度薄、視角廣、反應(yīng)速度快、可用于撓曲性面板、使用溫度范圍廣、構(gòu)造及制程較簡單等優(yōu)異之特性，被認為是下一代的平面顯示器新興應(yīng)用技術(shù)。OLED顯示技術(shù)具有自發(fā)光特性。它使用非常薄的有機材料涂層和玻璃基板。當(dāng)電流通過時，這些有機材料會發(fā)光，OLED顯示屏視角寬，可以省電。OLED顯示內(nèi)容如下：行數(shù)顯示參數(shù)表示內(nèi)容1Mode:Complementary互補濾波模式2Distance超聲波檢測到前方障礙的距離3EncoLeft左電機速度4EncoRight右電機速度5Angle車體傾斜角度6Battery電池電壓表1.2OLED顯示內(nèi)容由由于所需顯示內(nèi)容不多，我們使用0.96寸OLED，由SSD1306OLED驅(qū)動芯片進行驅(qū)動點亮，使用SPI接口進行通信。圖1.SEQ圖\*ARABIC11OLED顯示模塊電路圖圖1.SEQ圖\*ARABIC12OLED實物圖1.9小結(jié)綜上設(shè)計，系統(tǒng)必須用到的IO包括：（1）I2C數(shù)字接口，讀取MPU-6050數(shù)據(jù)；（2）PWM接口，控制左右兩個電極雙方向運行。由于采用單極性PWM驅(qū)動，需要四路PWM接口控制電機轉(zhuǎn)向；（3）電機PWM驅(qū)動接口：控制電機轉(zhuǎn)速；（4）串口下載UART接口2路，用于程序下載和調(diào)試接口；（5）Timer正交解碼接口：讀取編碼器的脈沖數(shù)據(jù):用于編碼器測速；（6）藍牙通訊UART接口除了以上確定使用的引腳，其它沒有占用的引腳全部預(yù)留出來，并且在底板上用兩排排針引出，用于連接其他模塊。綜上所述，平衡小車由STM32F103C8T6主芯片，由TB6612FNG電機驅(qū)動芯片驅(qū)動的TTM1直流減速電機，MPU-6050姿態(tài)傳感器，以及HC-SR04超聲波測距和0.96寸OLED等外設(shè)構(gòu)成。2系統(tǒng)軟件設(shè)計2.1軟件功能與框架本文的前幾章介紹了兩輪自平衡小車的硬件電路設(shè)計，平衡原理，控制策略，姿態(tài)檢測系統(tǒng)。本章將介紹兩輪自平衡小車的軟件設(shè)計，軟件設(shè)計是兩輪自平衡小車設(shè)計中的關(guān)鍵。軟件的主要功能包括有：（1）各傳感器信號的采集、處理；（2）電機PWM輸出；（3）車模運行控制：直立控制、速度控制、方向控制；（4）車模運行流程控制：程序初始化、車模啟動與結(jié)束、車模狀態(tài)監(jiān)控；（5）車模信息顯示與參數(shù)設(shè)定：狀態(tài)顯示、上位機監(jiān)控、參數(shù)設(shè)定等。上述功能可以分成兩大類：第一類包括1-3功能，它們屬于需要精確時間周期執(zhí)行，因此可以在一個周期定時中斷里完成。第二類包括4-5功能。它的執(zhí)行不需要精確的時間周期。可以放在程序的主程序中完成。這兩類任務(wù)之間可以通過全局變量實現(xiàn)相互的通訊[19][20]。根據(jù)系統(tǒng)要求，需要完成的總體軟件設(shè)計包括：單片機初始化，姿態(tài)信息采集，卡爾曼濾波，速度檢測，直流電機PID控制算法等。主程序框架如圖2.1所示。圖2.1主程序框架圖程序上電并執(zhí)行后，單片機被初始化。初始化工作包括兩部分，第一部分是初始化應(yīng)用于單片機的模塊。第二部分是應(yīng)用程序初始化，即對小車模型控制程序中使用的變量的值進行初始化。初始化完成后，首先進入車身豎立檢測程序。該程序通過讀取加速度計的值來判斷機體是否處于可控角度范圍的狀態(tài)。一旦在可控范圍內(nèi)，車身直立控制、速度控制和藍牙遙控被啟動。程序不斷地將監(jiān)控數(shù)據(jù)送入主循環(huán)，并通過串口發(fā)送給上位機進行監(jiān)控。同時檢查車模是否掉落。跌倒判斷可以通過汽車模型的傾斜角度是否超過一定范圍來確定。車體脫落后，停止車體的操作，包括車體直立控制、速度控制和轉(zhuǎn)向控制，然后繼續(xù)進行車體直立判斷過程。垂直車身控制、速度控制和藍牙遙控都是在中斷程序中完成的。全局標(biāo)志變量用于確定是否執(zhí)行這些閉環(huán)檢查。主程序如下：intmain(void){ BspInit(); //初始化BSP PIDInit(); //初始化PID CarUpstandInit(); //初始化系統(tǒng)參數(shù) SysTick_Init(); //初始化定時器 while(1) { SecTask(); //秒級任務(wù) if(SoftTimer[1]==0) {//每隔20ms執(zhí)行一次 SoftTimer[1]=20; ResponseIMU(); //上報姿態(tài)數(shù)據(jù) DebugService(); //上位機 Parse(Uart3Buffer);//APP } if(SoftTimer[2]==0) { SoftTimer[2]=20; ShowHomePage();//oled主頁刷新 Read_Distane();//觸發(fā)一次超聲波測距，并讀取上次測量結(jié)果 if(g_CarRunningMode==ULTRA_FOLLOW_MODE){ if(IsUltraOK()) UltraControl(0); //超聲波跟隨模式 } elseif(g_CarRunningMode==ULTRA_AVOID_MODE){ if(IsUltraOK()) UltraControl(1); //超聲波避障模式 } } } }2.2初始化程序上電并執(zhí)行后，單片機被初始化。初始化工作包括兩部分，第一部分是初始化應(yīng)用于單片機的模塊。第二部分是應(yīng)用程序初始化，即對小車模型控制程序中使用的變量的值進行初始化。其中單片機的初始化，包括GPIO初始化、PWM初始化、定時器初始化、串口初始化等。圖2.2上電初始化初始化代碼主要是硬件底層驅(qū)動和系統(tǒng)參數(shù)的初始化。下面我們來講解一下功能函數(shù)的具體作用。BspInit主要是硬件底層驅(qū)動的初始化，包括STM32外設(shè)的初始化、外圍模塊比如MPU-6050的初始化。/*bsp初始化*/voidBspInit(void){ SWDConfig(); ADCInit(); //ADC初始化USART1Init(); //串口1初始化-底板預(yù)留下載及調(diào)試用 USART3Init(0); //串口3初始化-用于藍牙TIM1_Cap_Init(); //TIM1初始化-用于超聲波跟隨功能 TIM3_PWM_Init(); //PWM初始化 TIM2_Encoder_Init(); //TIM2正交解碼初始化-用于測速 TIM4_Encoder_Init(); //TIM4正交解碼初始化-用于測速 i2cInit(); //I2C初始化 InfraredIOInit(); //紅外IO口初始化 OLED_Init(); //OLED初始化 delay_ms(20); MPU6050_Init(); //MPU6050初始化 LEDInit(); //指示燈初始化 UltraSelfCheck(); // 超聲模塊開機自檢 delay_ms(500); //延時0.5s，等待藍牙模塊啟動 Uart3SendStr("AT+BAUD8\r\n");//配置藍牙串口波特率為115200 delay_ms(20); Uart3SendStr("AT+RESET\r\n");//藍牙復(fù)位 USART3Init(1); //更改UART3波特率為115200 delay_ms(20);//延時 SetBlueToothName(); //配置藍牙模塊名稱}2.3中斷服務(wù)函數(shù)控制程序中最關(guān)鍵的部分，就是200Hz(5ms)中斷服務(wù)函數(shù)。車模的直立控制、速度控制以及方向控制都是在中斷服務(wù)函數(shù)中完成。通過全局標(biāo)志變量確定是否進行這些閉環(huán)控制。因為大部分定時器都另有他用，于是使用STM32的滴答定時器，產(chǎn)生1毫秒定時中斷。中斷服務(wù)程序的任務(wù)被均勻分配在0~4的中斷片段中，所有中斷片段全部執(zhí)行一遍需要5ms。因此每個中斷片段中的任務(wù)執(zhí)行的頻率為200Hz，即5ms。將任務(wù)分配到不同的中斷片段中，一方面防止這些任務(wù)累積執(zhí)行時間超過1ms，擾亂1ms中斷的時序，同時也考慮到這些任務(wù)之間的時間先后順序。當(dāng)然，也可以將所有的中斷任務(wù)都合并在一起，使用一個5ms的定時中斷來完成。這些任務(wù)包括：（1）電機測速脈沖計數(shù)器讀取與清除。累積電機轉(zhuǎn)動角度。累積電機速度，為后面速度控制提供平均數(shù)。（2）讀取傳感器的加速度數(shù)據(jù)、陀螺儀數(shù)據(jù)。（3）直立控制過程，包括姿態(tài)角度計算、直立控制計算。（4）速度控制過程，在這個1ms時間片段中，又進行0~5計數(shù)。在其中第0片段中，進行速度PID調(diào)節(jié)。因此，速度調(diào)節(jié)的周期為5×5=25毫秒，也就是每秒鐘調(diào)節(jié)40次。（5）方向控制過程，根據(jù)APP或上位機控制指令，計算偏差數(shù)值，然后計算電機差速控制輸出。（6）電機使能與失能控制，包括提起識別（懸空檢測）、著陸檢測、跌倒檢測等。（7）電機輸出，融合直立控制、速度控制、方向控制疊加值，最終賦予電機輸出。以上每個中斷任務(wù)都占用不同的時間，設(shè)定一個1ms定時中斷，讓這些任務(wù)分配在各自的1ms內(nèi)執(zhí)行，劃分了0~4共5個1ms時間片段，全部任務(wù)執(zhí)行一遍需要5ms。在程序調(diào)試時，需要保證它們都不會超過1ms。每個任務(wù)具體執(zhí)行時間可以通過示波器測量單片機IO引腳電平翻轉(zhuǎn)得到，設(shè)定執(zhí)行某任務(wù)前單片機的指定IO引腳為低電平，執(zhí)行完任務(wù)后，引腳電平翻轉(zhuǎn)為高電平。圖2.3中斷流程圖執(zhí)行完初始化，代碼會由于滴答定時器SysTick進入中斷而轉(zhuǎn)跳到SysTick定時中斷服務(wù)函數(shù)中執(zhí)行。中斷服務(wù)函數(shù):voidSysTick_Handler(void){ SoftTimerCountDown(); //軟定時器 g_u8MainEventCount++; g_u8SpeedControlPeriod++; SpeedControlOutput(); //速度環(huán)控制輸出函數(shù)，每1ms執(zhí)行一次 if(g_u8MainEventCount>=5) { g_u8MainEventCount=0; GetMotorPulse(); //捕獲電機脈沖（速度）函數(shù)，每5ms執(zhí)行一次 } elseif(g_u8MainEventCount==1) { MPU6050_Pose(); //讀取MPU6050數(shù)據(jù)函數(shù)，每5ms執(zhí)行一次 AngleCalculate(); //角度環(huán)計算函數(shù)，每5ms執(zhí)行一次 } elseif(g_u8MainEventCount==2) { AngleControl(); //角度環(huán)控制函數(shù)，每5ms執(zhí)行一次 } elseif(g_u8MainEventCount==3) { g_u8SpeedControlCount++; if(g_u8SpeedControlCount>=5)//25ms { SpeedControl();//車模速度控制函數(shù)，每25ms調(diào)用一次 g_u8SpeedControlCount=0; g_u8SpeedControlPeriod=0; } } elseif(g_u8MainEventCount==4) { MotorManage(); //電機使能/失能控制函數(shù)，每5ms執(zhí)行一次 MotorOutput(); //電機輸出函數(shù)，每5ms執(zhí)行一次 }}代碼中SoftTimerCountDown是軟定時器在SysTick中斷中得到精確的遞減。g_u8MainEventCount是主事件計數(shù)變量。需要在中斷中運行的代碼，在1ms內(nèi)運行不完，所以需要主事件計數(shù)變量將代碼合理分段，放到5個1ms內(nèi)運行。g_u8SpeedControlPeriod是速度環(huán)的控制周期計數(shù)變量。在5ms內(nèi)執(zhí)行GetMotorPulse、MPU6050_Pose、AngleCalculate、AngleControl、MotorManage、MotorOutput，而速度環(huán)是25ms運行一次。在中斷退出之后，程序繼續(xù)回到主程序往下執(zhí)行，完成藍牙通信和監(jiān)控調(diào)試等工作。自平衡兩輪車使用200Hz的刷新率來感知和調(diào)整自身狀況。對于數(shù)字系統(tǒng)，刷新率越高，控制越精確。但是主程序的刷新率也要結(jié)合MCU的計算速度（程序執(zhí)行效率）合理制定，否則會導(dǎo)致姿態(tài)檢測混亂等問題。2.4主要控制函數(shù)（1）輸入信號采集函數(shù)輸入信號采集函數(shù)讀取MPU-6050的三軸加速度傳感器數(shù)據(jù)，和傾倒軸對應(yīng)的陀螺儀數(shù)據(jù)（此處為陀螺儀Y軸）。voidSampleInputVoltage(void){/*此處不作任何濾波處理*/g_fGyro_y=DataSynthesis(GYRO_YOUT_H);//陀螺儀Y軸g_fAccel_x=DataSynthesis(ACCEL_XOUT_H);//加速度X軸g_fAccel_y=DataSynthesis(ACCEL_YOUT_H);//加速度Y軸g_fAccel_z=DataSynthesis(ACCEL_ZOUT_H);//加速度Z軸}（2）捕獲電機脈沖函數(shù)捕獲電機脈沖函數(shù)首先讀出T3T4寄存器數(shù)據(jù)，得到速度原始值，然后通過判斷前一次PWM輸出值來判斷電機的正反轉(zhuǎn)狀態(tài)，接著對80毫秒內(nèi)的脈沖進行累加。voidGetMotorPulse(void){/*讀出T3T4寄存器數(shù)據(jù)，得到速度原始值*/g_iRightMotorPulse=(T4H<<8)+T4L;g_iLeftMotorPulse=(T3H<<8)+T3L;T4T3M&=0x77;T4H=T4L=0;T3H=T3L=0;T4T3M|=0xCC;/*通過前一次PWM輸出判斷電機正反轉(zhuǎn)*/if(!MOTOR_LEFT_SPEED_POSITIVE)g_iLeftMotorPulse=-g_iLeftMotorPulse;if(!MOTOR_RIGHT_SPEED_POSITIVE)g_iRightMotorPulse=-g_iRightMotorPulse;/*捕獲電機脈沖函數(shù)每8毫秒執(zhí)行一次，速度環(huán)控制函數(shù)80毫秒執(zhí)行一次，因此先把脈沖累積起來*/g_iLeftMotorPulseSigma+=g_iLeftMotorPulse;g_iRightMotorPulseSigma+=g_iRightMotorPulse;}（3）直立計算函數(shù)直立計算函數(shù)用讀取到的加速度傳感器和陀螺儀數(shù)值除于各自的靈敏度，然后通過三角函數(shù)計算車模的傾角，再經(jīng)過卡爾曼濾波得到精準(zhǔn)的實時傾角。當(dāng)MPU-6050內(nèi)部加速度傳感器測量范圍是±2g時,測量精度為16384LSB/g，陀螺儀測量范圍是±2000°/s時，靈敏度為16.4gLSBs/°c。voidAngleCalculate(void){g_fAccel_x=g_fAccel_x/16384;g_fAccel_y=g_fAccel_y/16384;g_fAccel_z=g_fAccel_z/16384;g_fGyro_y=g_fGyro_y/16.4;g_fGyro_y*=-1;g_fGravityAngle=atan(g_fAccel_x/sqrt(g_fAccel_y*g_fAccel_y+g_fAccel_z*g_fAccel_z))*57.2957