教你做一個屬于自己的平衡小車，用STM32F103單片機實作（代碼篇）

平衡小車軟體篇

平衡小車硬體篇

目錄

一、定時器相關代碼

1、TIM2

2、TIM3和TIM4

二、中斷相關代碼

三、電機相關代碼

四、OLED螢屏和MUP6050相關代碼

五、PID函式

1、直立環

2、速度環

3、轉向環

六、控制函式

七、擴展篇

八、效果展示

說明：本篇文章適用于有點STM32單片機基礎，和有相關的硬體基礎，并且想做一個小專案的人，可以作為一個參考，當然了沒有基礎的也可以

如果需要代碼的話可以私聊我，供參考

下面就進行我們的代碼部分

一、定時器相關代碼

我這里使用了三個定時器，分別是TIM2,TIM3,TIM4，定時器2用于產生兩路PWM波，用來控制小車的速度，TIM3和TIM4用來對編碼器的脈沖進行計數，實作電機的倍訓控制，

1、TIM2

#include "pwm.h"

void PWM_Init_TIM2(u16 psc,u16 arr)
{
	GPIO_InitTypeDef GPIO_InitPwm;        //GPIO初始化
	TIM_TimeBaseInitTypeDef TIM_PwmInit;  //定時器初始化
	TIM_OCInitTypeDef TIM_OcPwminit;      //指定定時器輸出通道初始化
	
	RCC_APB2PeriphClockCmd(RCC_APB2Periph_GPIOA, ENABLE); //配置GPIO時鐘
	RCC_APB1PeriphClockCmd(RCC_APB1Periph_TIM2, ENABLE);

	GPIO_InitPwm.GPIO_Mode  = GPIO_Mode_AF_PP; //推完復用輸出
	GPIO_InitPwm.GPIO_Pin   = GPIO_Pin_2 | GPIO_Pin_3;
	GPIO_InitPwm.GPIO_Speed = GPIO_Speed_50MHz;
	GPIO_Init(GPIOA, &GPIO_InitPwm);

	TIM_PwmInit.TIM_CounterMode = TIM_CounterMode_Up; 
	TIM_PwmInit.TIM_Period = arr;
	TIM_PwmInit.TIM_Prescaler = psc;
	TIM_PwmInit.TIM_ClockDivision = TIM_CKD_DIV1; //1分頻
	TIM_TimeBaseInit(TIM2, &TIM_PwmInit);

	TIM_OcPwminit.TIM_OCMode      = TIM_OCMode_PWM1 ;       //比較輸出模式pwm1
	TIM_OcPwminit.TIM_OutputState = TIM_OutputState_Enable; //比較輸出使能 
	TIM_OcPwminit.TIM_OCNPolarity = TIM_OCNPolarity_Low;    //互補輸出極性、低電平有效

	TIM_OC3Init(TIM2, &TIM_OcPwminit);            //初始化通道
	TIM_OC4Init(TIM2, &TIM_OcPwminit);            //初始化通道

	TIM_OC3PreloadConfig(TIM2, TIM_OCPreload_Enable); 
	TIM_OC4PreloadConfig(TIM2, TIM_OCPreload_Enable); //控制波形是立即生效還是定時器發生下一次更新事件時被更新的
														//Enable:下一次更新事件時被更新
														//Disable:立即生效
	TIM_Cmd(TIM2, ENABLE);                      //使能TIM3的外設

}

由代碼可知PWM由定時器2的通道3和通道4輸出，對應到單片機引腳圖就是PA2和PA3，這里要注意，

2、TIM3和TIM4

TIM3和TIM4配置都差不多，就介紹一個就可以了，定時器配置代碼如下

#include "encoder.h"

void Encoder_TIM3_Init(void)
{
	GPIO_InitTypeDef GPIO_InitStruct;
	TIM_TimeBaseInitTypeDef TIM_TimeBaseInitStruct;
	TIM_ICInitTypeDef TIM_ICInitStruct;
	
	RCC_APB2PeriphClockCmd(RCC_APB2Periph_GPIOA,ENABLE);//開啟時鐘
	RCC_APB1PeriphClockCmd(RCC_APB1Periph_TIM3,ENABLE);
	
	GPIO_InitStruct.GPIO_Mode=GPIO_Mode_IN_FLOATING;//初始化
	GPIO_InitStruct.GPIO_Pin=GPIO_Pin_6 |GPIO_Pin_7;
	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(TIM3,TIM_EncoderMode_TI12,TIM_ICPolarity_Rising,TIM_ICPolarity_Rising);//配置編碼器模式
	
	TIM_ICStructInit(&TIM_ICInitStruct);//初始化輸入捕獲
	TIM_ICInitStruct.TIM_ICFilter=10;
	TIM_ICInit(TIM3,&TIM_ICInitStruct);
	
	TIM_ITConfig(TIM3,TIM_IT_Update,ENABLE);//配置溢位更新中斷標志位
	TIM_SetCounter(TIM3,0);//清零定時器計數值
	TIM_Cmd(TIM3,ENABLE);//開啟定時器
}

這里要注意有一個配置為編碼器模式的函式，函式的具體用法和函式引數什么意思就不在這里解釋了，可以去查詢相關資料或者后面出文章解釋

TIM_EncoderInterfaceConfig(TIM3,TIM_EncoderMode_TI12,TIM_ICPolarity_Rising,TIM_ICPolarity_Rising);
	

下面就是定時器中斷函式和讀取編碼器資料的函式了

/*********************
編碼器
速度讀取函式
入口引數：定時器幾
**********************/
int Read_Speed(int TIMx)
{
	int value_1;
	switch(TIMx)
	{                            //單位時間內,位移就是速度
		case 3:value_1=(short)TIM_GetCounter(TIM3);TIM_SetCounter(TIM3,0);break;
                                //1.采集編碼器的計數值并保存，2.將定時器的計數值清零，
		default:value_1=0;
	}
	return value_1;
}

void TIM3_IRQHandler(void)
{
	if(TIM_GetITStatus(TIM3,TIM_IT_Update)!=0){

		TIM_ClearITPendingBit(TIM3,TIM_IT_Update);
	}
}

速度讀取函式中，第一步采集定時器的計數值，也就是編碼器的資料，并保存，第二步將定時器清零，

二、中斷相關代碼

針對中斷這部分代碼有兩種解決方法，也可以用定時器實作，現在就介紹用中斷實作方法，其實原理都是一樣的，

#include "exti.h"

void MPU6050_EXTI_Init(void)    //中斷后有一個下降沿
{
	EXTI_InitTypeDef EXTI_InitStruct;
	GPIO_InitTypeDef GPIO_InitStruct;
	
	RCC_APB2PeriphClockCmd(RCC_APB2Periph_GPIOB | RCC_APB2Periph_AFIO,ENABLE);//開啟時鐘
	
	GPIO_InitStruct.GPIO_Mode=GPIO_Mode_IPU;
	GPIO_InitStruct.GPIO_Pin=GPIO_Pin_5;//PB5配置為上拉輸入
	GPIO_InitStruct.GPIO_Speed=GPIO_Speed_50MHz;
	GPIO_Init(GPIOB,&GPIO_InitStruct);	
	
	GPIO_EXTILineConfig(GPIO_PortSourceGPIOB,GPIO_PinSource5);//映射
	
	EXTI_InitStruct.EXTI_Line=EXTI_Line5;
	EXTI_InitStruct.EXTI_LineCmd=ENABLE;
	EXTI_InitStruct.EXTI_Mode=EXTI_Mode_Interrupt;
	EXTI_InitStruct.EXTI_Trigger=EXTI_Trigger_Falling;
	EXTI_Init(&EXTI_InitStruct);
}

這里配置了一個單片機引腳為外部中斷，(PB5)為什么要配置這個引腳呢？這個引腳和MPU6050的中斷引腳INT相連，INT是陀螺儀的中斷引腳，當采集一次資料時，就會發生一次中斷，對于中斷后干嘛，在后面說明，

三、電機相關代碼

#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_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);	
}

這個函式也就是配置4個引腳為輸出引腳，和電機相連

#include "motor.h"
/*賦值函式*/
void Load(int moto1,int moto2)
{
	                          //資料正負號，對應正反轉
	if(moto1>0)	Ain1=1,Ain2=0;//正轉
	else 		Ain1=0,Ain2=1;//反轉
	                          //加載PWM值
	TIM_SetCompare3(TIM2,GFP_abs(moto1)); //輸出

	if(moto2>0)	Bin1=1,Bin2=0;
	else 		Bin1=0,Bin2=1;	
	TIM_SetCompare4(TIM2,GFP_abs(moto2));
}

這個函式是用于將PID計算輸出的最終值加載到電機上，完成電機的控制，

四、OLED螢屏和MUP6050相關代碼

針對這兩部分的代碼我都是用的官方的示例代碼，這個網上都有很多的，陀螺儀只要能讀到資料就可以了，

五、PID函式

平衡小車的PID函式由三部分構成，分別為直立環，速度環，轉向環三部分構成，直立環就是讓小車角度趨近0，速度環就是讓電機速度趨近0，轉向環就是讓電機保持角速度為零，

1、直立環

/*********************
直立環PD控制器：Kp*Ek+Kd*Ek_D
入口：期望角度、真實角度、真實角速度
出口：直立環輸出
*********************/
int Vertical(float Med,float Angle,float gyro_Y)
{
	int PWM_out;
	
	PWM_out=Vertical_Kp*(Angle-Med)+Vertical_Kd*(gyro_Y-0); //gyro_Y-0 ：角速度偏差
	return PWM_out;
}

直立環采用的是PD控制器，即out = Kp*Ek+Kd*Ek_D，Ek_D表示偏差的微分

2、速度環

/*********************
速度環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;  //低通濾波系數
	Encoder_Err = ((encoder_left+encoder_right)-Target);	//計算速度偏差
	//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;//防止速度過大的影響直立環的正常作業，
	Encoder_S += EnC_Err_Lowout;//對速度偏差積分，積分出位移
	Encoder_S = Encoder_S > 10000 ? 10000 : (Encoder_S < (-10000) ? (-10000) :Encoder_S);//積分限幅
	PWM_out=Velocity_Kp*EnC_Err_Lowout+Velocity_Ki*Encoder_S;//速度環控制輸出計算
	return PWM_out;    //輸出
}

速度環采用的是PI控制器，即out = Kp*Ek+Ki*Ek_S，Ek_S表示偏差的積分

3、轉向環

/*********************
轉向環：系數*Z軸角速度
*********************/
int Turn(int gyro_Z)
{
	int PWM_out;
	PWM_out=(-0.6)*gyro_Z;    
	//	PWM_out=Turn_Kp*gyro_Z;
	return PWM_out;
}

轉向環比較簡單，輸出就是一個系數乘以Z軸角速度就可以了

六、控制函式

代碼如下

void EXTI9_5_IRQHandler(void)         //表示10ms中斷時間到
{
	int PWM_out;
	if(EXTI_GetITStatus(EXTI_Line5)!=0){//一級判定,看陀螺儀有沒有中斷
		
		if(PBin(5)==0){//二級判定   看看有沒有資料  低電平

			EXTI_ClearITPendingBit(EXTI_Line5);//清除中斷標志位
			//采集編碼器資料
			Encoder_Left=-Read_Speed(3);
			Encoder_Right=Read_Speed(4);
			//采集陀螺儀角度資訊
			mpu_dmp_get_data(&Pitch,&Roll,&Yaw);		//角度
			MPU_Get_Gyroscope(&gyrox,&gyroy,&gyroz);	//陀螺儀
			MPU_Get_Accelerometer(&aacx,&aacy,&aacz);	//加速度
			//將資料壓入倍訓控制中，計算出控制輸出量，
			Velocity_out=Velocity(Target_Speed,Encoder_Left,Encoder_Right);	//速度環			
			Vertical_out=Vertical(Velocity_out+Med_Angle,Pitch,gyroy);		//直立環    
			Turn_out=Turn(gyroz);											//轉向環							
			
			PWM_out=Vertical_out;//最終輸出
			//把控制輸出量加載到電機上，完成最終的的控制，
			MOTO1=PWM_out-Turn_out;
			MOTO2=PWM_out+Turn_out;
			Limit(&MOTO1,&MOTO2);	 //限幅			
			Load(MOTO1,MOTO2);		 //加載到電機上
		}
	}
}

這里就應該和前面PB5中斷函式聯系起來了，這里我利用的是陀螺儀采集完資料會產生中斷，(這里設定陀螺儀采集資料的周期為10ms)，然后就觸發單片機中斷，進入中斷完成相應的控制，

(當然了，這里也可以不用陀螺儀中斷，直接采用定時器定時10ms,然后進入定時器中斷函式完成相應的控制，所以這里是非常靈活的，)

其實這個控制函式也是比較簡單的，進入中斷后先清除中斷標志位，再讀取陀螺儀和編碼器的資料，將資料再壓入PID控制器進行計算，計算完成后將相應的值加載到電機上就可以了，進入中斷調整一次小車的姿態，一直不斷的調整，這樣就大概完成了平衡小車的制作，

七、擴展篇

如果有人想在小車上添加一些功能該怎么辦呢？比如實作超聲波避障，跟隨，或者是實作藍牙控制小車

當然了這也是可以實作的了，在速度環函式的入口引數中有一個Target_Speed變數就是期望速度，可以將這個變數單獨拿出來作為二次開發介面，然后改變這個變數的值來進行小車的移動，

比如下面就是實作藍牙控制的一部分代碼

MPU_Get_Accelerometer(&aacx,&aacy,&aacz);	//加速度
/*前后*/
if((Fore==0)&&(Back==0))Target_Speed=0;//速度清零，穩在原地
if(Fore==1)Target_Speed-=2;//需要前進
if(Back==1)Target_Speed+=2;//
Target_Speed=Target_Speed>SPEED_Y?SPEED_Y:(Target_Speed<-SPEED_Y?(-SPEED_Y):Target_Speed);//限幅
/*左右*/
if((Left==0)&&(Right==0))Turn_Speed=0;
if(Left==1)Turn_Speed+=30;	//左轉
if(Right==1)Turn_Speed-=30;	//右轉
Turn_Speed=Turn_Speed>SPEED_Z?SPEED_Z:(Turn_Speed<-SPEED_Z?(-SPEED_Z):Turn_Speed);//限幅( (20*100) * 100   )
/*轉向約束*/
if((Left==0)&&(Right==0))Turn_Kd=-0.6;//沒有左右轉向指令，則轉向約束
else if((Left==1)||(Right==1))Turn_Kd=0;//收到左右轉向指令，去掉轉向約束
Velocity_out=Velocity(Target_Speed,Encoder_Left,Encoder_Right);	//速度環
            

如果想要添加超聲波也是類似的方法，都是改變期望速度Target_Speed

八、效果展示