【嵌入式設計】【炒雞詳細】STM32單片機控制機器人程序設計框架解讀（不定時更新）

因爲疫情原因，我小機器人的底層單片機代碼沒人搞了，沒人弄了就得我自己上。碩士時候有點兒基礎，現在一邊兒做一邊兒學，爭取用一天時間把機器人的底層STM32代碼給搞出來。

雨哥最NB的地方就是學東西和做東西都很快，其中的原因就是雨哥一般是一邊兒做事一邊兒總結，在學習的時候順便把工作幹了，在工作的時候順便把知識學了。所以建議各位老鐵們多總結，不總結，今天學點兒東西明天就全忘了。

複習的時候寫一個博客，把心得跟大家共享：

1. STM32單片機在一臺智能車中擔任的角色

作爲一臺AppleZhang的小型智能車（差速輪）的協處理器，單片機的作用就是接受上面工控機的控制信號來驅動各個外設，同時將自己讀取的外設信號上傳到頂層工控機上。

我的差速小車的底層控制器有幾個功能需求：

兩路PWM驅動+4個GPIO（PWM需要用到單片機中的PWM輸出模塊，四個GPIO用來控制機器人的輪子方向）
兩個編碼器讀取的模塊（用來讀取機器人輪胎的轉動角度，用到單片機定時器的編碼器/計數器模式）
加速度傳感器模塊（我的是MPU6050，也就是需要IIC通訊）
四個超聲波傳感器（需要一個定時器，4個GPIO做Trigger，4個外部中斷做Echo）
一個步進電機控制（機器人轉頭）
串口通訊（跟上位機通訊，接受上位機控制信號）
兩個GPIO，用來控制兩個燈

2. PWM和GPIO用來控制機器人的輪胎旋轉速度和方向

第一步，我們先使用PWM來控制輪胎，使輪胎能夠按照我們的需求進行定功率旋轉：這一個過程需要幾個GPIO參與：

四個配置爲普通推輓輸出的GPIO【用於控制輪胎方向】
兩個配置爲PWM的GPIO【用於控制輪胎PWM】

2.1 配置輪胎的GPIO

我們用幾組引腳來驅動輪胎，PA15和PB3用來驅動左輪前進後退，PB4和PB6用來驅動右輪前進後退；配置普通GPIO輸出的方法比較簡單，一共分爲4個步驟：

1. 打開負責這個GPIO的APB時鐘 2. 定義一個GPIO_InitTypedef的變量 3. 設置這個變量（Pin，Mode，Speed） 4. 使用GPIO_Init(...)函數，對GPIO進行配置；代碼如下：

非常重要：在STM32F103系列單片機中，以上四個GPIO默認配置爲JTAG調試引腳，所以在機器人控制中會出現GPIO不受控或輸出錯誤的情況，此時，我們要顯式的將JTAG模式關閉；代碼如下：

RCC_APB2PeriphClockCmd(RCC_APB2Periph_AFIO, ENABLE);
GPIO_PinRemapConfig(GPIO_Remap_SWJ_JTAGDisable,ENABLE);

2.2 配置PWM定時器

STM32單片機想要輸出PWM波形，需要配置以下幾個內容：

1.打開GPIO使能開關，GPIO引腳爲複用輸出並使能

2.選擇一個Timer（例如TIM1），打開使能開關，構建並配置TIM_TimeBaseInitTypedef結構體

在TIM_TimeBaseInitTypedef這個結構體中有幾個參數比較關鍵，分別爲Prescaler， Period，ClockDivision，和Counter_Mode,它們依次代表預分頻數量（越高計數的速度越慢，設爲0就是主頻），計數週期（計數到多少會停止並觸發溢出中斷），時鐘週期（設置爲0就行），計數模式Counter_Mode分爲三種：向上，向下和中央對齊模式，可用下面的圖來表示（圖轉自下面參考鏈接）

3.構建一個TIM_OCInitTypedef（Output Configure）結構體，並配置到定時器通道上

在這個結構體中，有以下幾個關鍵參數，分別爲：TIM_OCMode, TIM_OutputState,TIM_Pulse, TIM_OC_Polarity;第一個參數代表輸出模式，跟Polarity一起用，意義就是告訴單片機在什麼時候輸出高。當OCMode=TIM_OCMode_PWM1，TIM_OC_Polarity=TIM_OC_Polarity_Low時，當Timer計數器值<CCRX時輸出低，反之輸出高。TIM_OutputState的值設置爲TIM_OutputState_Enable就可以；而Pulse的值爲預先裝載的數值，即CCRX的數值，這個設置爲0就可以，後續如果要改變PWM佔空比的話還得再調。

【注意：一個通道被配置爲PWM，其他通道也只能配置爲PWM，不可做其他用途了】

4.使能Timer的PWM輸出功能，各個通道的預裝載功能

5.使能Timer

6.通過配置寄存器的CCRX，就能控制PWM的輸出了。

參考了平衡小車之家的部分內容（文末有鏈接）在我這臺小機器人上使用了Timer1的CH1和CH4作爲輸出；配置代碼如下：

3. 正交編碼器讀取機器人的輪胎數據以及使用PID調速

光定功率旋轉輪子對一個機器人來講肯定不行，所以在這一個小節，我們要使用編碼器結合PID算法對機器人進行定速控制。

3.1 使用兩個timer的編碼器模式讀取輪胎數據

stm32的定時器中具有編碼器模式，編碼器模式依賴AB相正交編碼器（我小機器人上的編碼器）首先對其進行初始化：

1. 首先對Timer和GPIO進行初始化，步驟跟PWM的初始化一樣。

2. 使用TIM_EncoderInterfaceConfig(TIMX,MODE,IC1,IC2)將timer配置爲正交編碼器捕獲模式。其中，mode就是爲了正交編碼器而配置的，這裏有幾個圖講解了它的原理，其中mode是控制響應哪一跟線的編碼，共有三種模式。IC1是選擇A，B相的觸發邊沿，如果配置mode爲TIM_EncoderMode_T12(兩根線都響應)，IC1和2都配置爲TIM_ICPolarity_BothEdge，那麼也就是說任何一個相的升降都會引起編碼器數值變化，其中2根線，每個線1個上升+1個下降，即其數值爲單個編碼器單邊的4倍。

TIM的編碼器模式通過判斷某一根線電位變化時另一根線的電平來確定電機是正轉還是反轉。

3. 配置TIM_ICInitTypeDef實例的各項參數，包括濾波參數，濾波參數如下圖：

4. 開啓Timer整體配置

5. 編寫一個Read_Encoder()程序用來獲取編碼器的信息

完整的編碼器初始化程序如下圖：

編碼器讀取程序如圖（注意，爲了保證符號的準確性，在讀取的時候要將TIMX的16位寄存器強制轉換爲int16，即short類型）：

3.2 獲取輪子每個脈衝對應的移動距離

我使用ST-Link實時觀測變量發現，我這個小輪子從0開始轉，轉一圈兒，累計增加的數值爲0x331~0x33A；將它變爲10進製爲：0x331=817 0x33A=826;我們就取個整數820。

輪子轉一圈兒的距離很容易計算： $l=d\times \pi$

一圈計時器累加值爲n，則每個計數對應的輪子移動距離爲

即 $ss=d\times\pi/n$ 在我的小車裏，這個值爲：0.07*3.14/820= $2.68\times10^{-4}$ ,即0.28毫米。

別說，我10塊錢買這個小輪兒還挺好的。

3.3 使用PID算法對車輪進行速度調節（一般採用的方法，但會抖動）

很多工程控制都會用到PID，PID的思路很簡單，假設我們開車，想把車維持在60km/s的速度上，那麼車速如果只有十幾而且不加速，那麼了我們就要往死裏踩油門，當車子嗖一下子竄出去了，我們就得鬆油門；如果車速是58，59的樣子，那麼我們就輕輕，或者幾乎不踩油門。如果速度是65，那麼我們就鬆開油門，如果速度是120，那麼我們就得猛踩剎車。

這就是PID算法中的P因子。我們控制車速，參考的就是當前的速度和目標速度，如果這兩個速度差的很大，那麼我們就多給點兒油速度小，我們就少給點兒油。我們踩油門的程度就大致符合：

油門=（目標車速-當前車速）X一個固定的力道（Propotion）

這就是PID控制中的P（Percentage）。使用這種方式控制輪子的方式可以稱作P算法，現在我們把它實現一下：

在這個程序裏，與P值的代碼就三句（紅框），第一句：計算當前速度與目標速度的差值；第二句：差值乘上一個比例因子；第三句，返回這個結果。這個程序有三個輸入，分別爲當前速度，目標速度以及最大油門。返回值就是基於當前速度和目標速度計算出的油門數值。

但是在生活中我遇到過這樣一個問題：我有一輛小摩托，在我啓動小摩托後，爲了達到我想要的速度，我肯定是給足了油門，但是此時我的小摩托嗖一下就竄出去了（有一個很大的加速度），騎鬼火的社會人比較喜歡這種加速感，但生活不是鬼火，畢竟車頭一翹閻王爺笑。當我發現我的小鬼火要翹頭，我得趕快把油門鬆開。

此時我速度還沒達到目標速度，油門還得捏着（P比例因子告訴我要捏住油門），但是因爲我不能加速太快了，所以我還得鬆點兒油門。此時在P因子外，又有一種因子影響了油門，它就是：

微分比例因子（Derivative）

P因子和D因子在一起就構成了PD算法：

油門=P*（目標速度-當前速度）+D*(當前速度-上一時刻的速度)

參考了D值，剛剛的算法我們可以改爲：

PID算法中，我們已經掌握了P因子和D因子，當然，這這樣可能會導致另一個問題：我上一秒的速度是0，因爲加速太快導致第二秒的速度一下子升高到了59km/h，按照常理，按照P因子作用，我此時應該不踩油門了（因爲目標速度-當前速度≈0），但是由於D因子的存在（D因子發現我加速太快了），我會猛踩剎車。最後結果就是我速度到59了，踩剎車，速度下降，加速，又踩剎車；這樣也會達到目標速度，但是這種加速是非常不穩定的。

所以，我們綜合了微分，積分與比例因子，構建了完整的PID函數：

$E=P(v_tar-v0)+D*(\frac{dV}{dt})+I(2v0-\frac{dV}{dt}*\delta t))$

3.3-2使用改良的積分累加法對機器人進行控制

剛纔的算法問題在於PID很難調，一般情況下輪子都面臨着很強的抖動。所以我們換個思路來思考控制速度的問題（當然不是說PID不行，這個只是提供了一種更穩定的控制方法，具體採用哪種大家自行思考）

之前我們思考的邏輯，或者說數學模型是關於速度和油門之間的關係，速度矩陣是自變量，踩油門的力道是一個因變量。現在我們自變量不變，還是速度矩陣，而因變量我們變爲踩油門力道的變化值，用一個公式來表示：

$[v{_1},v{_2},...]\rightarrow \frac{dE}{dt}$

公式的左邊，就是踩油門力度的增量。舉個例子，按照PID算法，如果我們速度值很低，而目標速度很高，那麼我們就要通過：

$E=P(v_tar-v0)+D*(\frac{dV}{dt})+I(2v0-\frac{dV}{dt}*\delta t))$

這就必然會導致我們將油門踩到底，而這種控制，對車來講是很恐怖的。

現在，我們不直接配置E（油門量）值，而是使用一個參數對油門的變化量進行調節：

$E=E+P*\delta v+D*\delta(\delta v)$

我們就構建了這樣一個十分簡單的代碼：

當然，對這個模型的處理需要一些數學公式和建模的推導，能我後續有時間會專門發博客來進行建模。但是從我目前小機器人的運行狀態情況來看，這個模型所控制輪胎的穩定性要超越所有PID，它的不足之處就在於超低速狀態下（低於0.05m/s以下），輪胎速度的控制性不好，出現了動一下停一下的現象。

3.4 搭建定頻處理函數（機器人控制循環函數）

在上面的小節中我們提到了一個機器人速度，我們目前已經獲得了機器人編碼器的數值，但是在單片機中，我們很難知道機器人行走了x米的距離花了多長時間。所以，爲了讓機器人能夠準確的獲得自己的速度值，我們就要參考一個小學學到的公式：

（速度=路程/時間）

現在路程我們可以獲得了，至於時間，我們就要通過定時器獲得。

我們在前面已經用掉了單片機的Timer1（PWM輸出），Timer2和Timer4（獲取編碼器）。現在STM32F103C8T6這個單片機中還有兩個定時器可以選用：

Systick定時器
Timer3

我們使用Timer3來獲取中的t值。

Timer3的初始化跟之前一樣，而且timer3實現的是timer的基本功能，不需要GPIO的參與。我們只需要爲timer3設置好時鐘週期，同時設置好延時即可。

初始化代碼如下：

比較關鍵的一個地方是中斷的處理函數，我們在這個函數中讀取輪胎的實際速度（在這個函數中，我們可以控制一個小燈，實現對CPU佔用的顯示，在一個定頻處理週期中，CPU佔用的時間越長，燈就越亮）：

接下來我們就可以多主控制函數進行編寫了，可以把讀取速度和速度控制的函數都放進來：

重要！注意：我們在計算速度時所採用的數據類型爲浮點型（float或double），在定義比例因子宏的時候（例如PULSE_PERMETER），我們最好將數據定位爲浮點，如3700.0，3700f，否則整形和整形相乘，單片機會默認數據爲整形而忽略掉小數點，造成速度始終爲0，或在0，1之間跳變。

4. IIC與加速度傳感器信息的讀取

5. 使用串口與上層上位機通訊

單片機端串口的底層邏輯如下：

配置串口
配置串口中斷，同時完成一個底層程序用於解析數據字段
確認收到一個數據包後，操作動作，同時向上位機回傳相關數據。

接下來我們一步一步看：

5.1 初始化USART串口

STM32的發送和接收是通過數據寄存器USART_DR來實現的，這是一個雙寄存器，包含了TDR和RDR。當向該寄存器寫數據時，串口就會自動發送；當收到數據時，也存在該寄存器中。void USART_SendData(...)的意思就是想USART_DR寄存器寫入數據。USART_ReceiveData(...)相反。

串口狀態32位寄存器USART_SR【State Register】反映了串口的狀態，它的各個位代表的內容如下圖：

使用這個函數，就可以獲取串口寄存器各個位的數值：

FlagStatus USART_GetFlagStatus(USART_TypeDef* USARTx, uint16_t USART_FLAG);

這裏關注兩位：Bit5：RXNE；Bit6：TC
RXNE(讀數據寄存器非空)：當該位被置一時，說明已經有數據被接收了，並且可以讀出來了。此時應儘快讀取USART_DR。
讀取USART_DR或向該位寫0，都可以清除該位。
TC(發送完成)：當該位被置位時，說明USART_DR中的數據已經被髮送完成了。如果設置了這個位的中斷，則會產生中斷。
清零該位的兩種方法：a、讀取USART_SR，寫USART_DR。b、直接向該位寫0。

初始化串口很簡單，一共分爲三個步驟：

配置串口引腳，STM32單片機串口相關的引腳定義如下圖，我們使用串口1，則配置PA9(TX)爲GPIO_Mode_Out_PP,PA10(RX)爲GPIO_Mode_IN_Floating【注意：不要忘記打開GPIO的APB2時鐘開關！！】
配置串口參數，串口參數包括：BaudRate波特率，HardwareFlowControl（硬件控制），USART_Mode（TX還是RX還是TX|RX），USART_Parity(奇偶校驗)，USART_StopBIts（停止位），USART_WordLength（字長）。【注意：除了波特率之外，其他的所有參數的數值都是有宏定義的，WordLength要用USART_WordLength_8b表示！】
開啓GPIO和串口使能，開啓USART接收中斷使能（如果使用RX功能的話）
配置NVIC串口中斷優先級，在這裏使用的是NVIC_InitTypeDef結構體，需要配置的參數爲：1） NVIC_IRQChannel=USART1_IRQn; 2）主優先級IRQChannelPreemptionPriority，3）從優先級NVIC_IRQChannelSubPriority；4）中斷使能NVIC_IRQChannelCmd=ENABLE
NVIC_Init(&NVIC_InitStructure); 使能串口中斷及優先級。
在串口中斷函數void USART1_IRQHandler(void)中寫入處理代碼。

完整的初始化代碼如下圖：

5.2 編寫串口數據底層處理函數

現在我們初始完成了串口，我們就要進行數據的交互與處理了。

如果我們想讓單片機與上位機進行通訊，我們就必須要使用某種協議，爲了構建單片機與上位機都能夠理解的通訊信息，我們提出一個基於串口的通訊協議，我們就把它稱爲AppleZhang協議。

這個協議是一個半雙工的協議，首先，上位機向單片機發送一個數據段請求，然後，下位機基於上位機的請求，輸出相應數據：

因爲串口傳輸數據的時候可能會出現誤碼，所以我們要適當改良AppleZhang協議，添加一個誤碼重置機制：

完整的單片機端的底層串口處理函數如下（注意標出顏色的爲關鍵函數）：

5.3 編寫上層串口處理函數

在這裏我們來寫一個程序作爲例子：

在這裏有一個很有意思的現象，注意我畫框的地方；由於STM32單片機爲小端模式，所以低位在前高位在後，例如，我encoder的數值爲10（0x0000000a）,那麼memcpy後賦值出的4個Byte爲 0a 00 00 00。這個在解析的時候要着重處理，或者乾脆就一個字節一個字節的賦值。

由此，串口交互的框架就基本完成了，可以在這個框架中添加自己的上層協議，比如超聲波之類的，也可以向我一樣定義幾個宏：

6. GPIO以及周邊系統控制

6.1 步進電機控制方案

步進電機採用了28BYJ48，它一共有4個相，通過循環控制各個相線的高低就能夠驅動電機轉動，同時可以很方便的獲得角度。

我們採用4個引腳控制這個電機，分別爲B12，B13，B14，B15。這個電機可以用來控制機器人頭部的轉動，從而獲得更靈活的攝像頭視角。

我們採用定速的方式控制這個電機，首先，我們做一個變量存放這個電機當前旋轉的角度：然後再做一個變量存放電機的目標角度。同時，因爲這個電機有4個相線，我們要定義一個名爲phase的變量，告訴電機當前哪個相的位置被激活。

然後對GPIO進行初始化：

編寫一個控制頭部轉動的程序，原理就是根據目標位置和當前位置的差異，控制頭部旋轉：

然後在週期控制函數裏反覆調用這個轉頭的過程：

就可以了。

6.2 LED控制

這一部分沒什麼可說的，我的LED兩個引腳分別連到了PA2和PA3上，初始化一下，直接設置GPIO就好了。

6.3 通過AD轉換讀取電池電量

電池電量的讀取可以依賴於電池電壓（雖然直接讀電壓不準，但是電壓基本能反映電量而且比較方便）。

以12V鋰離子電池爲例，磷酸鐵鋰電池的電壓一般在11.3~12.6V之間，我們將12.6V計爲100，代表滿電；將11.3V計爲0，代表沒電。我們就可以構建一個簡單的線性模型：

, Bat代表電量百分比，V代表電池電壓。

原則上我們把電池電壓和電量的關係帶入函數：

就可以算出來了。但是實際上，單片機只能讀取3V以下的電壓，所以，我們要使用某種方法，先對電池電壓進行分壓後再進行操作。

電池電量的檢測硬件配置可以如下：

這樣的配置就將12V分壓了20/100倍爲2.4V。

單片機以內部參考的ADC有12位分辨率，以最大量程爲3.3V爲例，其測量電壓範圍在3.3/（0xfff）=3.3/4095=0.805mV

11.3V/5=2260mV,12.6V/5=2520mV, 所以測量電壓的最高分辨率爲：0.3%，這個也是符合預期的，因爲在電壓測量上，電池本身硬件的誤差率就會超過10%左右。

下面我們來進行ADC的軟件配置：

單片機ADC的啓動大致分爲以下幾個過程：

1. 根據ADC通道配置單片機的GPIO，GPIO的GPIO_Mode要配置爲GPIO_MODE_AIN

2. 使能GPIO與ADC時鐘

RCC_APB2PeriphClockCmd(RCC_APB2Periph_GPIOA |RCC_APB2Periph_ADC1 , ENABLE );

3. 配置ADC_InitTypedef結構體信息，包括：

ADC_Mode 設置爲ADC_Mode_Independent使多個ADC獨立工作，最常用的配置
ADCScanCovMode多通道轉換模式，如果僅使用了一個ADC通道，則設置爲DISABLE，使用了多通道，則設置爲ENABLE
ADC_ContinuousConvMode，DISABLE和ENABLE的區別在於ENABLE時轉換直到所有的數據轉換完成後才停止轉換，而DISABLE則只轉換一次數據就停止，要再次觸發轉換纔可以。
DC_ExternalTrigConv，外部觸發，一般設置爲軟件觸發：ADC_ExternalTrigConv_None
ADC_DataAlign，低位在哪邊兒，一般設置爲ADC_DataAlign_Right
NbrOfChannel 開啓幾個轉換通道，因爲檢測電池電壓只有1路，所以就設置爲1，否則設置的多一點兒。

4. ADC去初始化（ADC_DeInit()）

5. ADC和GPIO初始化（ADC_Init和GPIO_Init）

6. 校準ADC

完整初始化代碼如下：