零基础制作平衡小车【连载】13---平衡小车代码讲解（附源码）_平衡小车烧一次代码之后还用管吗

前言

今天聊一聊代码，只有直立功能的代码。

代码总体思路

给定一个目标值，单片机通过IIC和mpu6050通信，得知数据后，根据角度环计算出一个PWM值给电机驱动器，从而控制单机转动。电机转动，编码器就会输出脉冲，单片机通过编码器模式检测脉冲值，并把该值代入速度环计算出一个角度值送给角度环，角度环在输出PWM给电机，以此循环。其实就是上节的框图，再贴一遍。

程序

程序部分为了减少篇幅，之前讲解的编码器模式及pwm等代码都不在讲解，原理都一样，只是引脚不同，我只把重点代码说一下。如果想看这部分代码的同学，后面我整理之后会放在公众号。

1. 时序

通过外部中断检测mpu6050INT引脚，检测到信号之后就进行一次pid运算。以此来严格控制PID计算周期。可能有些人会有疑问，为什么不放到while中循环检测？因为放在while循环的话，这个循环是可以被中断打断的。小车平衡对实时性要求高，如果在while循环里，姿态矫正时，程序被其他模块中断，小车就立不起来了。因此外部中断的优先级要配置成最高。

代码如下：

/************************
外部中断，通过mpu6050的INT引脚，严格控制PID计算周期    PB5引脚
**************************/
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中断/事件线选择，可选 EXTI0 至 EXTI19
    
	EXTI_InitStruct.EXTI_LineCmd=ENABLE;//使能
    
	EXTI_InitStruct.EXTI_Mode=EXTI_Mode_Interrupt;//EXTI 模式选择
    
	EXTI_InitStruct.EXTI_Trigger=EXTI_Trigger_Falling;//EXTI 边沿触发事件
    
	EXTI_Init(&EXTI_InitStruct);//初始化中断
}

//优先级配置
void NVIC_Config(void)
{
	NVIC_InitTypeDef NVIC_InitStruct;
	
	NVIC_PriorityGroupConfig(NVIC_PriorityGroup_2);//4级抢占,4级响应。
	
	//外部中断
	NVIC_InitStruct.NVIC_IRQChannel=EXTI9_5_IRQn;
	NVIC_InitStruct.NVIC_IRQChannelCmd=ENABLE;
	NVIC_InitStruct.NVIC_IRQChannelPreemptionPriority=0;
	NVIC_InitStruct.NVIC_IRQChannelSubPriority=0;
	NVIC_Init(&NVIC_InitStruct);
}
1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42

2. 直立环

原理上节都讲过了，必要的地方我在程序中已经注释了。

/**************************************************************************
函数功能：直立PD控制
入口参数：角度、角速度
返回  值：直立控制PWM
作    者：平衡小车之家
**************************************************************************/
int balance(float Angle,float Gyro)
{  
	float Bias;
	int balance;
	Bias=Angle-ZHONGZHI;                       //===求出平衡的角度中值 和机械相关
	balance=Balance_Kp*Bias+Gyro*Balance_Kd;   //===计算平衡控制的电机PWM  PD控制   kp是P系数 kd是D系数 
	return balance;
}
1
2
3
4
5
6
7
8
9
10
11
12
13
14

上面角度环的公式是不是和上节中推导的一样。

Angle - Med_Angle：

Angle 为真实角度
Med_Angle为机械中值角度，也就是当小车直立时mpu6050所检测到的角度，该角度并不一定为0
Angle - Med_Angle为真实角度和机械中值角度的偏差

测量机械中值的方法（手动）：

将oled和mpu6050都连接好并调试通过之后，将采样的Pitch值显示在oled上。之后让小车轮子固定不动，用手沿电机旋转方向先逆时针（顺时针）慢慢推动小车上方，让小车直立，当小车从直立状态向逆时针方向倾斜时，看看屏幕上显示的角度是多少，之后同样的方法测顺时针，看看角度是多少，之后两值相加/2，得出的值就是机械中值。

3. 速度环

/**************************************************************************
函数功能：速度PI控制 修改前进后退速度，请修Target_Velocity，比如，改成60就比较慢了
入口参数：左轮编码器、右轮编码器
返回  值：速度控制PWM
作    者：平衡小车之家
**************************************************************************/
int velocity(int encoder_left,int encoder_right)
{  
     static float Velocity,Encoder_Least,Encoder,Movement;
	  static float Encoder_Integral,Target_Velocity;
   //=============速度PI控制器=======================//	
		Encoder_Least =(encoder_left+encoder_right)-0;                    //===获取最新速度偏差==测量速度（左右编码器之和）-目标速度（此处为零） 
		Encoder *= 0.7;		                                                //===一阶低通滤波器       
		Encoder += Encoder_Least*0.3;	                                    //===一阶低通滤波器    
		Encoder_Integral +=Encoder;                                       //===积分出位移 积分时间：10ms
		//Encoder_Integral=Encoder_Integral-Movement;                       //===接收遥控器数据，控制前进后退
		if(Encoder_Integral>10000)  	Encoder_Integral=10000;             //===积分限幅
		if(Encoder_Integral<-10000)	Encoder_Integral=-10000;              //===积分限幅	
		Velocity=Encoder*Velocity_Kp+Encoder_Integral*Velocity_Ki;        //===速度控制	
	  return Velocity;
}
1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21

速度环加入低通滤波，目的为了减少速度环的作用，毕竟直立才是最重要的。而且速度环相对于直立环来说是个干扰，因此不能让速度环作用太大。

4. 转向环

 /**************************************************************************
函数功能：转向控制  修改转向速度，请修改Turn_Amplitude即可
入口参数：左轮编码器、右轮编码器、Z轴陀螺仪
返回  值：转向控制PWM
作    者：平衡小车之家
**************************************************************************/
int turn(int encoder_left,int encoder_right,float gyro)//转向控制
{
	static float Turn_Target,Turn,Encoder_temp,Turn_Convert=0.9,Turn_Count;
	float Kd=1.3;     

	//=============转向PD控制器=======================//
	Turn=-gyro*Kd;                 //===结合Z轴陀螺仪进行PD控制
	return Turn;
}
1
2
3
4
5
6
7
8
9
10
11
12
13
14
15

转向环目的为了保持直线行走。
假设小车发生偏移，gyro就不为0，转向环就会有输出。

5. 最终控制代码

 int EXTI9_5_IRQHandler(void)
{
	int PWM_out;
	if(EXTI_GetITStatus(EXTI_Line5)!=0)//一级判定
	{
		if(PBin(5)==0)//二级判定
		{
			EXTI_ClearITPendingBit(EXTI_Line5);//清除中断标志位
			
			//进入中断首先读取数据----编码器数据和角度数据
			Encoder_Left=-Read_Encoder(2);               //===读取编码器的值
			Encoder_Right=Read_Encoder(4);              //===读取编码器的值
			
			mpu_dmp_get_data(&Pitch,&Roll,&Yaw);		//角度
			MPU_Get_Gyroscope(&gyrox,&gyroy,&gyroz);	//陀螺仪
			MPU_Get_Accelerometer(&aacx,&aacy,&aacz);	//加速度
			
			Balance_Pwm =balance(Pitch,gyroy);          //===角度环
			Velocity_Pwm=velocity(Encoder_Left,Encoder_Right);                  //===速度环PID控制	 记住，速度反馈是正反馈，就是小车快的时候要慢下来就需要再跑快一点
			Turn_Pwm =turn(Encoder_Left,Encoder_Right,gyroz);            	//===转向环PID控制     
			
			PWM_out=Balance_Pwm - Balance_Kp*Velocity_Pwm;//最终输出
			
			Moto1=PWM_out+Turn_Pwm;                            //===计算左轮电机最终PWM
			Moto2=PWM_out-Turn_Pwm;                            //===计算右轮电机最终PWM
			Xianfu_Pwm();                                                       //===PWM限幅
			Set_Pwm(Moto1,Moto2);                                               //===赋值给PWM寄存器  
		}
	}
	return 0;	
}
1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31

注意

PWM_out=Balance_Pwm - Balance_Kp*Velocity_Pwm;//最终输出
1

由于是串行PID，因此速度环的输出就是直立环的输入，如上面的框图一样。
因此将速度环代入到直立环当中，就可以求出最终输出。也就是将Velocity_Pwm代入到角度环形参Angle中即可得出最终输出公式。

6. mpu6050、oled代码移植
   这两个硬件相关的原理就不讲了，网上有很多视频讲解，这里简单说下移植步骤吧。
   第一步：找到相关的.c 和 .h文件
   
   
   将这几个文件复制到你的HAREWARE文件夹中，可以分别单独建个文件夹，方便查找。如下图：
   
   之后再keil中添加C文件及设置头文件查找路径：

之后进行编译，就能调用函数了。主函数初始化：

	OLED_Init();
    OLED_Clear();
	MPU_Init();
	mpu_dmp_init();
1
2
3
4

while中一直刷新平衡角：

	while(1)	
	{
		OLED_Float(0,10,Pitch,3);
	} 	
1
2
3
4

读取角度的函数放在外部中断中：

	mpu_dmp_get_data(&Pitch,&Roll,&Yaw);		//角度
	MPU_Get_Gyroscope(&gyrox,&gyroy,&gyroz);	//陀螺仪
	MPU_Get_Accelerometer(&aacx,&aacy,&aacz);	//加速度
1
2
3

总结

原理明白之后，在看代码是不是很简单，先在天上飘一会。。。

一路过来，其实平衡小车也不难嘛。再飘一会。。。

。。。。
下不来了。

代码的话放到公众号吧。
回复
“平衡小车代码-基础版”
领取代码。