前言

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

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

原理介紹

• 參考資料

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

• 系統框圖以及運動分析

系統框圖

獲取小車的速度和角度是實現小車自平衡的前提，數據在stm32的中斷控制中配合PID算法使用，輸出數值給PWM寄存器控制相應電機，從而實現平衡。遙控部分是控制轉向，根據移動終端向藍牙發送數據從而實現相應的運動。

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

運動分析

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

運動控制

小車的直立控制是通過負反饋控制的

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

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

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

轉向控制

?

?

?所謂轉向環即當小車已經能保證基本的平衡時，通過藍牙控制使小車改變方向的控制系統

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

PWM輸出

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

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

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

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

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

編碼器模式

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

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

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

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

參考手冊如下：

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

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

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

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

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

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

更詳細請參考編碼器配置原理

MPU-6050姿態解算

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

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

  參考資料 MPU-6050

PID

• PID算法

  上邊運動分析中已經提及小車的運動需要直立環速度環以及轉向環。PID的學習包括理論知識以及調參的經驗等。

  參考資料 PID算法原理、調試經驗和代碼z2r8

?

模塊選型

主控模塊

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

編碼器

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

額定電壓7.4V，當然也可以工作在12V環境下，編碼器供電5v。

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

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

電機驅動模塊

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

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

降壓模塊

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

MPU-6050模塊

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

藍牙通訊模塊

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

硬件準備

IO口分配

定時器TIM4預留給OLED 每10ms刷新一次數據

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

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

GPIO復用情況連接外設

PA2	USART2_RX	藍牙 TX
PA3	USART2_TX	藍牙 RX
PA0	TIM2_CH1	電機A編碼器A相
PA1	TIM2_CH2	電機A編碼器B相
PA6	TIM3_CH1	電機B編碼器B相
PA7	TIM3_CH2	電機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型接口，電機等處加入100nf電容進行濾波。

PCB繪制

PCB未鋪銅如圖所示

鋪銅以及添加淚滴效果

2D預覽

實物圖

硬件組裝

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

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

實物圖如下

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

軟件調試

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

配置編碼器

#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的每個跳變沿均計數 TIM_ICPolarity_Rising：不反相。 /***********************根據兩個輸入信號（TI1&TI2）的跳變順序，產生了計數脈沖和方向信號。 依據兩個輸入信號的跳變順序，計數器向上或向下計數，同時硬件對TIMx_CR1寄存器的DIR位進行相應的設置。 不管計數器是依靠TI1計數、依靠TI2計數或者同時依靠TI1和TI2計數。 在任一輸入端(TI1或者TI2)的跳變都會重新計算DIR位。【正反轉】 正轉：T1超前T2相位90度。 反轉：T1滯后T2相位90度。【模式】 TI1模式：在T1的所有邊沿計數。 TI2模式：在T2的所有邊沿計數。 TI12模式：在T1和T2的所有邊沿計數。*************************/TIM_ICStructInit(&TIM_ICInitStruct);//初始化輸入捕獲TIM_ICInitStruct.TIM_ICFilter=10;//濾波器TIM_ICInit(TIM2,&TIM_ICInitStruct);TIM_ITConfig(TIM2,TIM_IT_Update,ENABLE);//配置溢出更新中斷標志位TIM_SetCounter(TIM2,0);//清零定時器計數值TIM_Cmd(TIM2,ENABLE);//開啟定時器 }/********************** 編碼器 速度讀取函數 入口參數：定時器 **********************/ 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.采集編碼器的計數值并保存。2.將定時器的計數值清零。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); //清除中斷標志位} }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為復用推挽輸出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預裝載寄存器使能TIM_Cmd(TIM1,ENABLE);//使能定時器。 }

配置電機

#include "motor.h"/*電機初始化函數*/ 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; /*限幅函數*/ 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; }/*絕對值函數*/ int abs(int p) {int q;q=p>0?p:(-p);return q; }/*賦值函數*/ /*入口參數：PID運算完成后的最終PWM值*/ void Load(int moto1,int moto2)//moto1=-200：反轉200個脈沖 {//1.研究正負號，對應正反轉if(moto1>0) Ain1=1,Ain2=0;//正轉else Ain1=0,Ain2=1;//反轉//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模塊無法進入中斷，所以預留的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); //根據設定參數初始化GPIOB }/*不同的管腳對應的修改的地址不一樣，這里需要注意*/#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

配置藍牙

藍牙預留USART2，在中斷程序中執行轉向程序

我們用的平衡之家的APP，不同的程序所對應的代碼不同，都為16進制數，自己查詢軟件指令即可。

//GPIO初始化 void USART2_init(u32 bound) {//GPIO端口設置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 初始化設置USART_InitStructure.USART_BaudRate = bound;//一般設置為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) //先嘗試能不能進中斷 不能再放外邊 {char Bluetooth_data;if(USART_GetITStatus(USART2,USART_IT_RXNE)!=RESET)//接收中斷標志位拉高{Bluetooth_data=USART_ReceiveData(USART2);//保存接收的數據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;//未接受到前進后退指令-->速度清零，穩在原地if(Fore==1)Target_Speed--;//前進1標志位拉高-->需要前進if(Back==1)Target_Speed++;///*左右*/if((Left==0)&&(Right==0))Turn_Speed=0;if(Left==1)Turn_Speed+=30; //左轉if(Right==1)Turn_Speed-=30; //右轉/*轉向約束*/if((Left==0)&&(Right==0))Turn_Kd=-0.8;//若無左右轉向指令，則開啟轉向約束else if((Left==1)||(Right==1))Turn_Kd=0;//若左右轉向指令接收到，則去掉轉向約束

PID

PID控制分為直立環以及速度環，轉向環大同小異不再展示

調參步驟：

確立機械中值。

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

直立環（內環）—Kp極性、Kp大小。Kd極性、Kd大小。

Kp極性：

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

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

Kp大小：

Kp一直增加，直到出現大幅低頻震蕩。

Kd極性：

極性錯誤：拿起小車繞電機軸旋轉，車輪反向轉動，無跟隨。

極性正確：拿起小車繞電機軸旋轉，車輪同向轉動，有跟隨。

Kd大小：

Kd一直增加，直到出現高頻震蕩。

直立環調試完畢后，對所有確立的參數乘以0.6作為最終參數。

速度環（外環）——Kp&Ki極性、Kp&Ki大小。

在調試【速度環參數極性】時：需要去掉（注釋掉）【直立環運算】

在調試【速度環參數大小】時：再次引入（取消注釋）【直立環運算】

ki/kp線性關系Ki=(1/200)*Kp、僅調Kp即可。

Kp&Ki極性：*

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

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

Kp&Ki大小：

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

對于PID具體調參 參考運動分析部分處

/******************** 入口：期望角度、真實角度、真實角速度 出口：直立環輸出 *********************/ 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; }/********************* 速度環PI：Kp*Ek+Ki*Ek_S // 入口 左電機速度, 右點擊速度 *********************/ 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.對速度偏差進行低通濾波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;//防止速度過大的影響直立環的正常工作。//3.對速度偏差積分，積分出位移Encoder_S+=EnC_Err_Lowout;//4.速度環控制輸出計算PWM_out=Velocity_Kp*EnC_Err_Lowout+Velocity_Ki*Encoder_S;return PWM_out;}

主程序初始化

我們主要實現的是小車直立，所以OLED以及傳感器不再展示

定義變量

float Med_Angle=2; //機械中值。floatVertical_Kp=-125 , //直立環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, //速度環KP、KI 1.11 1.14** 1.16 1.18Velocity_Ki=0.005; // Ki = 1/200 * Kpint MOTO1=0,MOTO2=0; // 電機裝載變量float Pitch,Roll,Yaw; //角度信息,俯仰角,翻滾角,偏航角 short gyrox,gyroy,gyroz;//陀螺儀角速度 short aacx,aacy,aacz;//加速度 int Encoder_Left,Encoder_Right;//編碼器數據(速度) 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(); //=====延時函數初始化LED_Init(); //=====LED初始化 程序燈I2C_Configuration(); //硬件I2C初始化OLED_Init(); //OLED初始化 Encoder_TIM2_Init(); //編碼器Encoder_TIM3_Init();IIC_Init(); //=====軟件IIC初始化 讀取MPU6050數據MPU6050_initialize(); //=====MPU6050初始化 DMP_Init(); //=====初始化DMP PWM_Init_TIM1(0,7199); //72M/(1) / (7200) = 10kHZdelay_ms(1000); //延時1s 解決小車上電輪子亂轉的問題Motor_Init();//電機TIM3_Configuration(); //超聲波拿來捕獲的定時器UltrasonicWave_Configuration(); //超聲波UltrasonicWave_StartMeasure(); //超聲波 BASIC_TIM_Init(); //TIM4基本定時器 只定時 更新數據用EXTI_Init(); //=====MPU6050 5ms定時中斷初始化

控制部分

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

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、采集編碼器數據&MPU6050角度信息。Encoder_Left=-Read_Speed(2);Encoder_Right=Read_Speed(3);//2、將數據壓入閉環控制中，計算出控制輸出量。Velocity_out=Velocity(Target_Speed,Encoder_Left,Encoder_Right); //速度環Vertical_out=Vertical(Velocity_out+Med_Angle,Pitch,gyroy); //直立環Turn_out=Turn(gyroz,Turn_Speed); PWM_out=Vertical_out;//最終輸出//3、把控制輸出量加載到電機上，完成最終的的控制。MOTO1=PWM_out;//左電機MOTO2=PWM_out;//右電機Limit(&MOTO1,&MOTO2); //PWM限幅 Load(MOTO1,MOTO2); //加載到電機上。Stop(&Med_Angle,&Pitch);//安全檢測

后言

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

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

總結

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

如果覺得生活随笔網站內容還不錯，歡迎將生活随笔推薦給好友。