前言

近年來，移動機(jī)器人是目前科學(xué)領(lǐng)域比較活躍的領(lǐng)域之一，其應(yīng)用范圍越來越廣泛，面臨的環(huán)境也越來越復(fù)雜，這就要求機(jī)器人能夠適應(yīng)一些復(fù)雜的環(huán)境和任務(wù)。二輪自平衡機(jī)器人正是在這一背景下提出來的，對于制作此種類型的自平衡小車無疑對我stm32的學(xué)習(xí)有莫大的好處，一方面深入了解stm32以及學(xué)習(xí)串級PID的運用，另一方面也是對我近期學(xué)習(xí)的硬件方面知識一個檢驗。

本人為32小白水平有限，同時也是第一篇博客，如果有地方描述不清晰不正確，歡迎大佬指出一起討論

原理介紹

• 參考資料

  第七屆全國大學(xué)生“飛思卡爾”杯智能汽車競賽 讀取碼gveh

• 系統(tǒng)框圖以及運動分析

系統(tǒng)框圖

獲取小車的速度和角度是實現(xiàn)小車自平衡的前提，數(shù)據(jù)在stm32的中斷控制中配合PID算法使用，輸出數(shù)值給PWM寄存器控制相應(yīng)電機(jī)，從而實現(xiàn)平衡。遙控部分是控制轉(zhuǎn)向，根據(jù)移動終端向藍(lán)牙發(fā)送數(shù)據(jù)從而實現(xiàn)相應(yīng)的運動。

我們可以將平衡小車的運動方式類比為手指上放置一根筷子，保持其直立狀態(tài)，根據(jù)生活經(jīng)驗可知，當(dāng)筷子向某個方向倒下時，我們眼睛觀察到筷子倒下反饋給大腦，大腦控制手迅速向筷子倒下的方向運動，使其始終保持原有直立狀態(tài)。同樣，平衡小車道理相同，當(dāng)傳感器檢測到小車有倒下的趨勢時，控制輪子向相同方向運動，不同角度會輸出不同速度即可保持平衡。

運動分析

平衡小車的控制可以分為直立控制，速度控制以及轉(zhuǎn)向控制，即直立環(huán)、速度環(huán)和轉(zhuǎn)向環(huán)，使小車能夠保持平衡狀態(tài)的是速度環(huán)以及直立環(huán)，通過傳感器讀取角度輸出不同的PWM即不同的速度使其保持平衡。其中單獨的直立環(huán)可以使小車保持直立，但如果受到外力的影響，那么不用多想，小車一定會倒下，將直立環(huán)和速度環(huán)串起來，那么當(dāng)小車受到外力時，也能迅速平衡。

運動控制

小車的直立控制是通過負(fù)反饋控制的

高中時期大家對正反饋和負(fù)反饋都有所了解，這里不過多解釋。直立控制實際上就是將小車控制在一定角度內(nèi)，這個角度由小車的機(jī)械零點有關(guān)（你搭建的小車重心），對于機(jī)械零點的確定會在調(diào)試中說明，上文提到如果只有直立環(huán)不足以小車完全保持平衡，所以我們還需要加入速度環(huán)。

小車的速度控制是通過正反饋控制的

我們可以假設(shè)此時小車處于直立狀態(tài)，如果小車想向前運動，那么小車必然要向前傾斜獲取向前的加速度，但是向前傾斜輪子必然要向后轉(zhuǎn)動，那么小車的速度會下降（輪子向后轉(zhuǎn)動了）假如為負(fù)反饋那么小車必然會增加傾角，一直循環(huán)下去這樣則會加速小車的倒下，所以在速度控制中反饋系統(tǒng)應(yīng)該是正反饋。

轉(zhuǎn)向控制

?

?

?所謂轉(zhuǎn)向環(huán)即當(dāng)小車已經(jīng)能保證基本的平衡時，通過藍(lán)牙控制使小車改變方向的控制系統(tǒng)

• 定時器PWM輸出以及編碼器模式

PWM輸出

脈沖寬度調(diào)制(PWM)，是英文“Pulse Width Modulation” 的縮寫，簡稱脈寬調(diào)制，是利用微處理器的數(shù)字輸出來對模擬電路進(jìn)行控制的一種非常有效的技術(shù)。

當(dāng)小車在運行過程中隨著傾角的改變小車的速度也應(yīng)該發(fā)生變化，如果只是單純給予高低電平，只能以額定速度運動，顯然不能達(dá)到我們想達(dá)到的程度，所以我們需要定時器輸出PWM波，我們選用的主控模塊只有4個定時器（1個高級3個通用）一個定時器有4個通道控制小車顯然是足夠的。當(dāng)然如果你選擇其他系列芯片會有多個定時器，選擇也會更多

（注意基本定時器無法輸出PWM脈沖）

其中如果用到高級定時器TIM1以及TIM8注意除了基礎(chǔ)的配置還需要使能剎車和死區(qū)寄存器，以使能整個PWM輸出，還需要使能高級定時器特有的寄存器。

TIM_CtrlPWMOutputs(TIM1,ENABLE); //高級定時器特有

編碼器模式

上文系統(tǒng)框圖可以看出，速度系統(tǒng)需要一個速度反饋，其他系統(tǒng)的反饋可以通過6050讀取，那么此時小車的速度如何讀取呢？這里就需要編碼器讀取此時的速度。編碼器會引出AB相，通過STM32的編碼器模式以特定的GPIO口連接編碼器AB相，讀取脈沖，從而讀取速度。

通過查閱參考手冊不難得知只有高級定時器以及通用定時器具有檢測編碼器的功能，基本定時器并不具備這種功能。所以我們可以配置定時器來讀取編碼器數(shù)據(jù)。

這里需要注意的是：編碼器模式只有定時器的通道一和通道二可以使用，即只有TIMx_CH1 TIMx_CH2 可以使用

千萬注意這一點，如果自己打板的話沒注意這一點就可以重新打板了 /(ㄒoㄒ)/

參考手冊如下：

選擇編碼器接口模式的方法是：如果計數(shù)器只在TI2的邊沿計數(shù)，則置TIMx_SMCR寄存器中的SMS=001；如果只在TI1邊沿計數(shù)，則置SMS=010；如果計數(shù)器同時在TI1和TI2邊沿計數(shù)，則置SMS=011。

通過設(shè)置TIMx_CCER寄存器中的CC1P和CC2P位，可以選擇TI1和TI2極性；如果需要，還可以對輸入濾波器編程。

兩個輸入TI1和TI2被用來作為增量編碼器的接口。參看下表：

假定計數(shù)器已經(jīng)啟動(TIMx_CR1寄存器中的CEN=’1’)，計數(shù)器由每次在TI1FP1或TI2FP2上的有效跳變驅(qū)動。 TI1FP1和TI2FP2是TI1和TI2在通過輸入濾波器和極性控制后的信號；如果沒有濾波和變相，則TI1FP1=TI1，TI2FP2=TI2。根據(jù)兩個輸入信號的跳變順序，產(chǎn)生了計數(shù)脈沖和方向信號。依據(jù)兩個輸入信號的跳變順序，計數(shù)器向上或向下計數(shù)，同時硬件對TIMx_CR1寄存器的DIR位進(jìn)行相應(yīng)的設(shè)置。不管計數(shù)器是依靠TI1計數(shù)、依靠TI2計數(shù)或者同時依靠TI1和TI2計數(shù)。在任一輸入端(TI1或者TI2)的跳變都會重新計算DIR位。

編碼器接口模式基本上相當(dāng)于使用了一個帶有方向選擇的外部時鐘。這意味著計數(shù)器只在0到TIMx_ARR寄存器的自動裝載值之間連續(xù)計數(shù)(根據(jù)方向，或是0到ARR計數(shù)，或是ARR到0計數(shù))。所以在開始計數(shù)之前必須配置TIMx_ARR；同樣，捕獲器、比較器、預(yù)分頻器、觸發(fā)輸出特性等仍工作如常。在這個模式下，計數(shù)器依照增量編碼器的速度和方向被自動的修改，因此計數(shù)器的內(nèi)容始終指示著編碼器的位置。計數(shù)方向與相連的傳感器旋轉(zhuǎn)的方向?qū)?yīng)。

當(dāng)定時器配置成編碼器接口模式時，提供傳感器當(dāng)前位置的信息。使用第二個配置在捕獲模式的定時器，可以測量兩個編碼器事件的間隔，獲得動態(tài)的信息(速度，加速度，減速度)。指示機(jī)械零點的編碼器輸出可被用做此目的。根據(jù)兩個事件間的間隔，可以按照固定的時間讀出計數(shù)器。如果可能的話，你可以把計數(shù)器的值鎖存到第三個輸入捕獲寄存器(捕獲信號必須是周期的并且可以由另一個定時器產(chǎn)生)；也可以通過一個由實時時鐘產(chǎn)生的DMA請求來讀取它的值。

更詳細(xì)請參考編碼器配置原理

MPU-6050姿態(tài)解算

• 六軸傳感器讀取角度
  想要小車直立起來由上文可知我們需要讀取y軸的傾斜角度以及y軸的加速度，當(dāng)然如果用到轉(zhuǎn)向環(huán)還要讀取z軸的角度。MPU-6050可讀取3軸的角度以及3軸的加速度和芯片的溫度。其中角度是直立環(huán)重要參數(shù)，角速度是速度環(huán)以及轉(zhuǎn)向環(huán)的重要參數(shù)。

  獲得6050的數(shù)據(jù)我們可以通過移植官方的DMP庫，通過DMP，就可以使用InvenSense公司提供的運動處理資料庫，非常方便地實現(xiàn)姿態(tài)解算，也可以獲得原始數(shù)據(jù)后使用卡爾曼濾波解算姿態(tài)。
  需要注意的是AD0接口如果接地，則6050的IIC地址為0x68，如果接vcc，則6050的IIC地址0x69

  參考資料 MPU-6050

PID

• PID算法

  上邊運動分析中已經(jīng)提及小車的運動需要直立環(huán)速度環(huán)以及轉(zhuǎn)向環(huán)。PID的學(xué)習(xí)包括理論知識以及調(diào)參的經(jīng)驗等。

  參考資料 PID算法原理、調(diào)試經(jīng)驗和代碼z2r8

?

模塊選型

主控模塊

主控模塊我們使用STM32f103c8t6 ，這款模塊有3個通用定時器，一個高級定時器，3路串口，2路IIC，72Mhz，對于我們的需求是綽綽有余的。

編碼器

編碼器電機(jī)我們使用的是平衡之家的一款mini電機(jī)，由于打出的板子體型較小，所以沒有選擇體積大的電機(jī)。

額定電壓7.4V，當(dāng)然也可以工作在12V環(huán)境下，編碼器供電5v。

價格差不了多少，還是體積大一點的搭建出來的小車看起來大氣

電機(jī)線1/6接在電機(jī)模塊的A/BOUT處，電機(jī)線2/5接5v以及接地，編碼器AB相接在預(yù)留出來的定時器接口只有通道1/2支持

電機(jī)驅(qū)動模塊

電機(jī)驅(qū)動模塊選擇的是TB6612模塊，VM需要12v供電，SYBT需要5v供電否則電機(jī)不會轉(zhuǎn)動， 同時輸出兩路PWM，同時控制兩個電機(jī)。

這里其實一開始選擇是功能更強(qiáng)大的A4950，但是因為摔了一下電機(jī)模塊就直接罷工（太便宜），所以在購置硬件時千萬不要圖便宜，買好的不買便宜的

降壓模塊

降壓模塊選擇一塊帶數(shù)顯的，方便觀察電池電量，不多敘述。

MPU-6050模塊

不多贅述，同樣6050也不要圖便宜（這一塊可能就是因為質(zhì)量問題，導(dǎo)致程序無法進(jìn)入6050的外部中斷，最后沒辦法放入主程序運行）

藍(lán)牙通訊模塊

轉(zhuǎn)向環(huán)則是實現(xiàn)小車轉(zhuǎn)彎的一環(huán)。通常利用上位機(jī)與平衡小車的交互來來控制轉(zhuǎn)向，這里使用的是藍(lán)牙模塊HC-06，HC-05/HC-06 /SPP-C這些通用的型號都可以用，藍(lán)牙模塊通過串口通信來傳輸數(shù)據(jù)。

硬件準(zhǔn)備

IO口分配

定時器TIM4預(yù)留給OLED 每10ms刷新一次數(shù)據(jù)

定時器TIM3用于測距通過OLED展示距離

另外注意編碼器只能用通道1/2

GPIO復(fù)用情況連接外設(shè)

PA2	USART2_RX	藍(lán)牙 TX
PA3	USART2_TX	藍(lán)牙 RX
PA0	TIM2_CH1	電機(jī)A編碼器A相
PA1	TIM2_CH2	電機(jī)A編碼器B相
PA6	TIM3_CH1	電機(jī)B編碼器B相
PA7	TIM3_CH2	電機(jī)B編碼器A相
PA8	TIM1_CH1	TB6612 PWMA
PA11	TIM1_CH4	TB6612 PWMB
PB1	TIM3_CH3	SR04 -> Trig
PB0	TIM3_CH4	SR04 -> Echo
PB5	無	6050外部中斷
PB6	IIC1 SCL	6050 SCL
PB7	IIC1 SDA	6050 SDA
PB10	IIC2 SCL	OLED SCL
PB11	IIC2 SDA	OLED SDA
PB12	無	BIN2
PB13	無	BIN1
PB14	無	AIN1
PB15	無	AIN2

原理圖繪制

原理圖如圖所示，供電接口采用的T型接口，電機(jī)等處加入100nf電容進(jìn)行濾波。

PCB繪制

PCB未鋪銅如圖所示

鋪銅以及添加淚滴效果

2D預(yù)覽

實物圖

硬件組裝

3S電池搭建在最底層，中間放置降壓模塊，銅柱搭建最上方PCB，防止電機(jī)運動時的抖動影響系統(tǒng)的運行。

其實這里是有問題的，預(yù)留的OLED位置不夠，導(dǎo)致只能放置在邊上，PCB打孔處和降壓模塊沖突，整體排布也影響重心，需要調(diào)試機(jī)械中值。

實物圖如下

硬件裝配完成，下面我們開始軟件編程

軟件調(diào)試

程序是刪減后的，并不完整 ，程序還是自己寫學(xué)習(xí)才更有意義😀

配置編碼器

#include "encoder.h"void Encoder_TIM2_Init(void) {GPIO_InitTypeDef GPIO_InitStruct;TIM_TimeBaseInitTypeDef TIM_TimeBaseInitStruct;TIM_ICInitTypeDef TIM_ICInitStruct;RCC_APB2PeriphClockCmd(RCC_APB2Periph_GPIOA,ENABLE);//開啟時鐘RCC_APB1PeriphClockCmd(RCC_APB1Periph_TIM2,ENABLE);GPIO_InitStruct.GPIO_Mode=GPIO_Mode_IN_FLOATING;//初始化GPIO--PA0、PA1GPIO_InitStruct.GPIO_Pin=GPIO_Pin_0 |GPIO_Pin_1;GPIO_Init(GPIOA,&GPIO_InitStruct);TIM_TimeBaseStructInit(&TIM_TimeBaseInitStruct);//初始化定時器。TIM_TimeBaseInitStruct.TIM_ClockDivision=TIM_CKD_DIV1;TIM_TimeBaseInitStruct.TIM_CounterMode=TIM_CounterMode_Up;TIM_TimeBaseInitStruct.TIM_Period=65535;TIM_TimeBaseInitStruct.TIM_Prescaler=0;TIM_TimeBaseInit(TIM2,&TIM_TimeBaseInitStruct);TIM_EncoderInterfaceConfig(TIM2,TIM_EncoderMode_TI12,TIM_ICPolarity_Rising,TIM_ICPolarity_Rising);//配置編碼器模式 T1和T2的每個跳變沿均計數(shù) TIM_ICPolarity_Rising：不反相。 /***********************根據(jù)兩個輸入信號（TI1&TI2）的跳變順序，產(chǎn)生了計數(shù)脈沖和方向信號。 依據(jù)兩個輸入信號的跳變順序，計數(shù)器向上或向下計數(shù)，同時硬件對TIMx_CR1寄存器的DIR位進(jìn)行相應(yīng)的設(shè)置。 不管計數(shù)器是依靠TI1計數(shù)、依靠TI2計數(shù)或者同時依靠TI1和TI2計數(shù)。 在任一輸入端(TI1或者TI2)的跳變都會重新計算DIR位。【正反轉(zhuǎn)】 正轉(zhuǎn)：T1超前T2相位90度。 反轉(zhuǎn)：T1滯后T2相位90度。【模式】 TI1模式：在T1的所有邊沿計數(shù)。 TI2模式：在T2的所有邊沿計數(shù)。 TI12模式：在T1和T2的所有邊沿計數(shù)。*************************/TIM_ICStructInit(&TIM_ICInitStruct);//初始化輸入捕獲TIM_ICInitStruct.TIM_ICFilter=10;//濾波器TIM_ICInit(TIM2,&TIM_ICInitStruct);TIM_ITConfig(TIM2,TIM_IT_Update,ENABLE);//配置溢出更新中斷標(biāo)志位TIM_SetCounter(TIM2,0);//清零定時器計數(shù)值TIM_Cmd(TIM2,ENABLE);//開啟定時器 }/********************** 編碼器 速度讀取函數(shù) 入口參數(shù)：定時器 **********************/ int Read_Speed(int TIMx) {int value_1;switch(TIMx){case 2:value=(short)TIM_GetCounter(TIM2);TIM_SetCounter(TIM2,0);break;//IF是定時器2，1.采集編碼器的計數(shù)值并保存。2.將定時器的計數(shù)值清零。case 3:value=(short)TIM_GetCounter(TIM3);TIM_SetCounter(TIM3,0);break;default:value=0;}return value; }void TIM2_IRQHandler(void) {if(TIM_GetITStatus(TIM2,TIM_IT_Update)!=0){TIM_ClearITPendingBit(TIM2,TIM_IT_Update); //清除中斷標(biāo)志位} }void TIM3_IRQHandler(void) {if(TIM_GetITStatus(TIM3,TIM_IT_Update)!=0){TIM_ClearITPendingBit(TIM3,TIM_IT_Update);} }

配置PWM輸出

#include "pwm.h"void PWM_Init_TIM1(u16 Psc,u16 Per) {GPIO_InitTypeDef GPIO_InitStruct;TIM_TimeBaseInitTypeDef TIM_TimeBaseInitStruct;TIM_OCInitTypeDef TIM_OCInitStruct;RCC_APB2PeriphClockCmd(RCC_APB2Periph_GPIOA | RCC_APB2Periph_TIM1,ENABLE);//開啟時鐘GPIO_InitStruct.GPIO_Mode=GPIO_Mode_AF_PP;//初始化GPIO--PA8、PA11為復(fù)用推挽輸出GPIO_InitStruct.GPIO_Pin=GPIO_Pin_8 |GPIO_Pin_11; GPIO_InitStruct.GPIO_Speed=GPIO_Speed_50MHz;GPIO_Init(GPIOA,&GPIO_InitStruct);TIM_TimeBaseStructInit(&TIM_TimeBaseInitStruct);//初始化定時器。TIM_TimeBaseInitStruct.TIM_ClockDivision=TIM_CKD_DIV1;TIM_TimeBaseInitStruct.TIM_CounterMode=TIM_CounterMode_Up;TIM_TimeBaseInitStruct.TIM_Period=Per;TIM_TimeBaseInitStruct.TIM_Prescaler=Psc;TIM_TimeBaseInit(TIM1,&TIM_TimeBaseInitStruct);TIM_OCInitStruct.TIM_OCMode=TIM_OCMode_PWM1;//初始化輸出比較 選擇PWM1模式TIM_OCInitStruct.TIM_OCPolarity=TIM_OCPolarity_High;TIM_OCInitStruct.TIM_OutputState=TIM_OutputState_Enable;TIM_OCInitStruct.TIM_Pulse=0;TIM_OC1Init(TIM1,&TIM_OCInitStruct);TIM_OC4Init(TIM1,&TIM_OCInitStruct);TIM_CtrlPWMOutputs(TIM1,ENABLE);//高級定時器專屬TIM_OC1PreloadConfig(TIM1,TIM_OCPreload_Enable);TIM_OC4PreloadConfig(TIM1,TIM_OCPreload_Enable);TIM_ARRPreloadConfig(TIM1,ENABLE);//TIM1預(yù)裝載寄存器使能TIM_Cmd(TIM1,ENABLE);//使能定時器。 }

配置電機(jī)

#include "motor.h"/*電機(jī)初始化函數(shù)*/ void Motor_Init(void) {GPIO_InitTypeDef GPIO_InitStruct;RCC_APB2PeriphClockCmd(RCC_APB2Periph_GPIOB,ENABLE);//開啟時鐘GPIO_InitStruct.GPIO_Mode=GPIO_Mode_Out_PP;//初始化GPIO--PB12、PB13、PB14、PB15為推挽輸出GPIO_InitStruct.GPIO_Pin=GPIO_Pin_12 |GPIO_Pin_13 |GPIO_Pin_14 |GPIO_Pin_15;GPIO_InitStruct.GPIO_Speed=GPIO_Speed_50MHz;GPIO_Init(GPIOB,&GPIO_InitStruct); }int PWM_MAX = 3000; int PWM_MIN = -3000; /*限幅函數(shù)*/ void Limit(int *motoA,int *motoB) {if(*motoA>PWM_MAX)*motoA=PWM_MAX;else if(*motoA<PWM_MIN)*motoA=PWM_MIN;if(*motoB>PWM_MAX)*motoB=PWM_MAX;else if(*motoB<PWM_MIN)*motoB=PWM_MIN; }/*絕對值函數(shù)*/ int abs(int p) {int q;q=p>0?p:(-p);return q; }/*賦值函數(shù)*/ /*入口參數(shù)：PID運算完成后的最終PWM值*/ void Load(int moto1,int moto2)//moto1=-200：反轉(zhuǎn)200個脈沖 {//1.研究正負(fù)號，對應(yīng)正反轉(zhuǎn)if(moto1>0) Ain1=1,Ain2=0;//正轉(zhuǎn)else Ain1=0,Ain2=1;//反轉(zhuǎn)//2.研究PWM值TIM_SetCompare1(TIM1,abs(moto1)); //等價直接賦值在->CRR寄存器if(moto2>0) Bin1=0,Bin2=1;else Bin1=1,Bin2=0; TIM_SetCompare4(TIM1,abs(moto2)); }char PWM_Zero=0,stop=0; void Stop(float *Med_Jiaodu,float *Jiaodu) {if(GFP_abs(*Jiaodu-*Med_Jiaodu)>50){Load(PWM_Zero,PWM_Zero);} }

配置MPU-6050

直接移植的DMP庫，為軟件IIC，所以只需要修改管腳以及宏定義即可

因為我這款6050模塊無法進(jìn)入中斷，所以預(yù)留的PB5外部中斷接口暫時沒用上，所以外部中斷的GPIO的初始化不再展示

void IIC_Init(void) { GPIO_InitTypeDef GPIO_InitStructure;RCC_APB2PeriphClockCmd(RCC_APB2Periph_GPIOB, ENABLE); //使能PB端口時鐘GPIO_InitStructure.GPIO_Pin = GPIO_Pin_6|GPIO_Pin_7; //端口配置GPIO_InitStructure.GPIO_Mode = GPIO_Mode_Out_PP; //推挽輸出GPIO_InitStructure.GPIO_Speed = GPIO_Speed_50MHz; //50MGPIO_Init(GPIOB, &GPIO_InitStructure); //根據(jù)設(shè)定參數(shù)初始化GPIOB }/*不同的管腳對應(yīng)的修改的地址不一樣，這里需要注意*/#define SDA_IN() {GPIOB->CRL&=0X0FFFFFFF;GPIOB->CRL|=8<<28;} #define SDA_OUT() {GPIOB->CRL&=0X0FFFFFFF;GPIOB->CRL|=3<<28;}#define IIC_SCL PBout(6) //SCL #define IIC_SDA PBout(7) //SDA #define READ_SDA PBin(7) //輸入SDA

配置藍(lán)牙

藍(lán)牙預(yù)留USART2，在中斷程序中執(zhí)行轉(zhuǎn)向程序

我們用的平衡之家的APP，不同的程序所對應(yīng)的代碼不同，都為16進(jìn)制數(shù)，自己查詢軟件指令即可。

//GPIO初始化 void USART2_init(u32 bound) {//GPIO端口設(shè)置GPIO_InitTypeDef GPIO_InitStructure;USART_InitTypeDef USART_InitStructure;NVIC_InitTypeDef NVIC_InitStruct; RCC_APB2PeriphClockCmd(RCC_APB2Periph_GPIOA, ENABLE);RCC_APB1PeriphClockCmd(RCC_APB1Periph_USART2,ENABLE); //USART2_TX GPIO_InitStructure.GPIO_Pin = GPIO_Pin_2;GPIO_InitStructure.GPIO_Speed = GPIO_Speed_50MHz;GPIO_InitStructure.GPIO_Mode = GPIO_Mode_AF_PP;GPIO_Init(GPIOC, &GPIO_InitStructure);//USART2_RX GPIO_InitStructure.GPIO_Pin = GPIO_Pin_3;GPIO_InitStructure.GPIO_Mode = GPIO_Mode_IPU;GPIO_Init(GPIOC, &GPIO_InitStructure); //USART 初始化設(shè)置USART_InitStructure.USART_BaudRate = bound;//一般設(shè)置為9600;USART_InitStructure.USART_WordLength = USART_WordLength_8b;USART_InitStructure.USART_StopBits = USART_StopBits_1;USART_InitStructure.USART_Parity = USART_Parity_No;USART_InitStructure.USART_HardwareFlowControl = USART_HardwareFlowControl_None;USART_InitStructure.USART_Mode = USART_Mode_Rx | USART_Mode_Tx;USART_Init(USART2, &USART_InitStructure);USART_ITConfig(USART2, USART_IT_RXNE, ENABLE);//開啟中斷USART_Cmd(USART2, ENABLE); //使能串口 NVIC初始化MY_NVIC_PriorityGroupConfig(2);NVIC_InitStruct.NVIC_IRQChannel=USART2_IRQn;NVIC_InitStruct.NVIC_IRQChannelCmd=ENABLE;NVIC_InitStruct.NVIC_IRQChannelPreemptionPriority=0;NVIC_InitStruct.NVIC_IRQChannelSubPriority=0;NVIC_Init(&NVIC_InitStruct); }u8 Fore,Back,Left,Right;void USART2_IRQHandler(void) //先嘗試能不能進(jìn)中斷 不能再放外邊 {char Bluetooth_data;if(USART_GetITStatus(USART2,USART_IT_RXNE)!=RESET)//接收中斷標(biāo)志位拉高{Bluetooth_data=USART_ReceiveData(USART2);//保存接收的數(shù)據(jù)if(Bluetooth_data==0x5A) Fore=0,Back=0,Left=0,Right=0;//剎else if(Bluetooth_data==0x41) Fore=1,Back=0,Left=0,Right=0;//前else if(Bluetooth_data==0x45) Fore=0,Back=1,Left=0,Right=0;//后//else if(Bluetooth_data==0x42) Fore=1,Back=0,Left=1,Right=0;//右前else if(Bluetooth_data==0x47) Fore=0,Back=0,Left=1,Right=0;//左else if(Bluetooth_data==0x43) Fore=0,Back=0,Left=0,Right=1;//右else Fore=0,Back=0,Left=0,Right=0;//剎}}//一個字符 void USART2_Send_Data(char data) {USART_SendData(USART2,data);while(USART_GetFlagStatus(USART2,USART_FLAG_TC)!=1); }//一串字符 void USART2_Send_String(char *String) {u16 len,j;len=strlen(String);for(j=0;j<len;j++){USART2_Send_Data(*String++);} }if((Fore==0)&&(Back==0))Target_Speed=0;//未接受到前進(jìn)后退指令-->速度清零，穩(wěn)在原地if(Fore==1)Target_Speed--;//前進(jìn)1標(biāo)志位拉高-->需要前進(jìn)if(Back==1)Target_Speed++;///*左右*/if((Left==0)&&(Right==0))Turn_Speed=0;if(Left==1)Turn_Speed+=30; //左轉(zhuǎn)if(Right==1)Turn_Speed-=30; //右轉(zhuǎn)/*轉(zhuǎn)向約束*/if((Left==0)&&(Right==0))Turn_Kd=-0.8;//若無左右轉(zhuǎn)向指令，則開啟轉(zhuǎn)向約束else if((Left==1)||(Right==1))Turn_Kd=0;//若左右轉(zhuǎn)向指令接收到，則去掉轉(zhuǎn)向約束

PID

PID控制分為直立環(huán)以及速度環(huán)，轉(zhuǎn)向環(huán)大同小異不再展示

調(diào)參步驟：

確立機(jī)械中值。

***機(jī)械中值：***把平衡小車放在地面上，從前向后以及從后向前繞電機(jī)軸旋轉(zhuǎn)平衡小車，兩次的向另一邊倒下的角度的中值，就是機(jī)械中值。

直立環(huán)（內(nèi)環(huán)）—Kp極性、Kp大小。Kd極性、Kd大小。

Kp極性：

極性錯誤：小車往哪邊倒，車輪就往反方向開，會使得小車加速倒下。

極性正確：小車往哪邊倒，車輪就往哪邊開，以保證小車有直立的趨勢。

Kp大小：

Kp一直增加，直到出現(xiàn)大幅低頻震蕩。

Kd極性：

極性錯誤：拿起小車?yán)@電機(jī)軸旋轉(zhuǎn)，車輪反向轉(zhuǎn)動，無跟隨。

極性正確：拿起小車?yán)@電機(jī)軸旋轉(zhuǎn)，車輪同向轉(zhuǎn)動，有跟隨。

Kd大小：

Kd一直增加，直到出現(xiàn)高頻震蕩。

直立環(huán)調(diào)試完畢后，對所有確立的參數(shù)乘以0.6作為最終參數(shù)。

速度環(huán)（外環(huán)）——Kp&Ki極性、Kp&Ki大小。

在調(diào)試【速度環(huán)參數(shù)極性】時：需要去掉（注釋掉）【直立環(huán)運算】

在調(diào)試【速度環(huán)參數(shù)大小】時：再次引入（取消注釋）【直立環(huán)運算】

ki/kp線性關(guān)系Ki=(1/200)*Kp、僅調(diào)Kp即可。

Kp&Ki極性：*

極性錯誤：手動轉(zhuǎn)動其中一個車輪，另一車輪會以同樣速度反向旋轉(zhuǎn)——典型負(fù)反饋。

極性正確：手動轉(zhuǎn)動其中一個車輪，兩個車倫會同向加速，直至電機(jī)最大速度——典型正反饋。

Kp&Ki大小：

增加Kp&Ki，直至：小車保持平衡的同時，速度接近于零。且回位效果較好。

對于PID具體調(diào)參 參考運動分析部分處

/******************** 入口：期望角度、真實角度、真實角速度 出口：直立環(huán)輸出 *********************/ int Vertical(float Med,float Angle,float gyro_y) {int PWM_out;PWM_out=Vertical_Kp*(Angle-Med)+Vertical_Kd*(gyro_y-0);return PWM_out; }/********************* 速度環(huán)PI：Kp*Ek+Ki*Ek_S // 入口 左電機(jī)速度, 右點擊速度 *********************/ int Velocity(int Target,int encoder_left,int encoder_right) {static int Encoder_S,EnC_Err_Lowout_last,PWM_out,Encoder_Err,EnC_Err_Lowout;float a=0.7;//1.計算速度偏差Encoder_Err=((encoder_left+encoder_right)-Target);//2.對速度偏差進(jìn)行低通濾波low_out=(1-a)*Ek+a*low_out_last;EnC_Err_Lowout=(1-a)*Encoder_Err+a*EnC_Err_Lowout_last;//使得波形更加平滑，濾除高頻干擾，防止速度突變。EnC_Err_Lowout_last=EnC_Err_Lowout;//防止速度過大的影響直立環(huán)的正常工作。//3.對速度偏差積分，積分出位移Encoder_S+=EnC_Err_Lowout;//4.速度環(huán)控制輸出計算PWM_out=Velocity_Kp*EnC_Err_Lowout+Velocity_Ki*Encoder_S;return PWM_out;}

主程序初始化

我們主要實現(xiàn)的是小車直立，所以O(shè)LED以及傳感器不再展示

定義變量

float Med_Angle=2; //機(jī)械中值。floatVertical_Kp=-125 , //直立環(huán)KP、KD 225 215* 220* 129 125** 115 Vertical_Kd=-0.765; //1.1 1.18 1.21* 1.25* 0.768 0.773** 0.764floatVelocity_Kp=1.0, //速度環(huán)KP、KI 1.11 1.14** 1.16 1.18Velocity_Ki=0.005; // Ki = 1/200 * Kpint MOTO1=0,MOTO2=0; // 電機(jī)裝載變量float Pitch,Roll,Yaw; //角度信息,俯仰角,翻滾角,偏航角 short gyrox,gyroy,gyroz;//陀螺儀角速度 short aacx,aacy,aacz;//加速度 int Encoder_Left,Encoder_Right;//編碼器數(shù)據(jù)(速度) int Vertical_out,Velocity_out,Turn_out;

初始化

int main(void) { //char buf[16];uart_init(115200);GPIO_PinRemapConfig(GPIO_Remap_SWJ_Disable,ENABLE);GPIO_PinRemapConfig(GPIO_Remap_SWJ_JTAGDisable,ENABLE);//禁用JTAG 啟用 SWDdelay_init(); //=====延時函數(shù)初始化LED_Init(); //=====LED初始化 程序燈I2C_Configuration(); //硬件I2C初始化OLED_Init(); //OLED初始化 Encoder_TIM2_Init(); //編碼器Encoder_TIM3_Init();IIC_Init(); //=====軟件IIC初始化 讀取MPU6050數(shù)據(jù)MPU6050_initialize(); //=====MPU6050初始化 DMP_Init(); //=====初始化DMP PWM_Init_TIM1(0,7199); //72M/(1) / (7200) = 10kHZdelay_ms(1000); //延時1s 解決小車上電輪子亂轉(zhuǎn)的問題Motor_Init();//電機(jī)TIM3_Configuration(); //超聲波拿來捕獲的定時器UltrasonicWave_Configuration(); //超聲波UltrasonicWave_StartMeasure(); //超聲波 BASIC_TIM_Init(); //TIM4基本定時器 只定時 更新數(shù)據(jù)用EXTI_Init(); //=====MPU6050 5ms定時中斷初始化

控制部分

控制部分包括將PID的最終輸出加載到電機(jī)上以及在串口上打印出讀取的角度以及角速度

while(1){int PWM_out;Read_DMP(); printf("%d\r\n",(int)Roll);printf("%d\r\n",(int)Yaw);printf("%d\r\n",(int)Pitch);printf("%d\r\n",gyrox);printf("%d\r\n",gyroy);printf("%d\r\n",gyroz);gyrox = gyro [0];gyroy = gyro [1];gyroz = gyro [2];aacx = accel[0];aacy = accel[1];aacz = accel[2];//1、采集編碼器數(shù)據(jù)&MPU6050角度信息。Encoder_Left=-Read_Speed(2);Encoder_Right=Read_Speed(3);//2、將數(shù)據(jù)壓入閉環(huán)控制中，計算出控制輸出量。Velocity_out=Velocity(Target_Speed,Encoder_Left,Encoder_Right); //速度環(huán)Vertical_out=Vertical(Velocity_out+Med_Angle,Pitch,gyroy); //直立環(huán)Turn_out=Turn(gyroz,Turn_Speed); PWM_out=Vertical_out;//最終輸出//3、把控制輸出量加載到電機(jī)上，完成最終的的控制。MOTO1=PWM_out;//左電機(jī)MOTO2=PWM_out;//右電機(jī)Limit(&MOTO1,&MOTO2); //PWM限幅 Load(MOTO1,MOTO2); //加載到電機(jī)上。Stop(&Med_Angle,&Pitch);//安全檢測

后言

到這里小車已經(jīng)基本實現(xiàn)直立以及外力影響下快速恢復(fù)，（當(dāng)然是你的PID調(diào)試十分順利情況下，一開始我是直接試數(shù)盲調(diào)，所以我的調(diào)試過程十分令人暴躁，學(xué)長建議看波形圖才逐漸調(diào)試好）現(xiàn)在回想調(diào)試過程中還是暴露出很多問題，包括6050無法進(jìn)入中斷，PCB打板出來無法使用等等問題。總的來說這次項目的獨立完成還是能學(xué)到很多東西，從最初的原理運動分析到模塊選型以及PCB學(xué)習(xí)和繪制，當(dāng)然這種單純繪制電路板還是不夠完美，繼續(xù)學(xué)習(xí)，爭取下次做項目能完成集成的PCB，當(dāng)然不局限于EDA，現(xiàn)在也在學(xué)習(xí)AD.由于時間緊迫以及個人能力有限，有地方可能表述不是很清楚，歡迎大佬們指出，歡迎在評論區(qū)提出來😀。

那么平衡小車的硬件以及軟件教程就到這里啦，我們下次再見😊！！

總結(jié)

以上是生活随笔為你收集整理的基于STM32的二轮自平衡小车的全部內(nèi)容，希望文章能夠幫你解決所遇到的問題。

如果覺得生活随笔網(wǎng)站內(nèi)容還不錯，歡迎將生活随笔推薦給好友。