基于stm32与陀螺仪（mpu6050）的PID角度环算法，角度用OLED显示，使得智能车能在长时间跑直线和转直角弯，减小误差

首先，我做智能车用的是stm32f103c8t6作为主控芯片，得到小车自身对于开始位置的三维变换角度所用的是mpu6050模块，其与主控芯片采用I2C通信。此通信原理接下来会加入介绍资料。其次还有一个OLED模块，这个也和mpu6050模块相似，都采用I2C与主控芯片之间进行通信。接下来我会分模块介绍原理，但是如果大家想直接使用mpu6050和OLED的驱动代码，在最后我除了会把PID算法的代码开源，还会将两个外设的驱动代码开源放在后面。

一.模块的作用

1.首先是stm32f103c8t6主控芯片作用是与两个外设进行通信。与mpu6050通信获得小车距离原位置的偏离量，与OLED通信使OLED屏幕上显示实时小车偏离量，当然，这个模块还有很多其他用处，接下来详细说。现在说主控芯片在接电压的时候不要接成5V，这样会直接烧坏主控芯片，所以要用规定的电压，比如我用的这个就是3.3V。

 2.mpu6050模块

1.可以数字输出6轴或9轴的旋转矩阵、四元数(quaternion)、欧拉角格式(Euler Angle forma)的融合演算数据。 具有131 LSBs/°/sec 敏感度与全格感测范围为±250、±500、±1000与±2000°/sec 的3轴角速度感测器(陀螺仪)。 可程式控制，且程式控制范围为±2g、±4g、±8g和±16g的3轴加速器。 移除加速器与陀螺仪轴间敏感度，降低设定给予的影响与感测器的飘移。 数字运动处理(DMP: Digital Motion Processing)引擎可减少复杂的融合演算数据、感测器同步化、姿势感应等的负荷。

 2.采用I2C通信，可以与众多支持该通信的微控制器之间相互使用，使用非常方便，在出厂时已经附带使用源代码，避免了新手小白不会写驱动问题。但是唯一的问题是该传感器的误差会随时间产生误差，大概1个小时就会产生近一百角度的误差（在放置不动的情况下），所以在后面我们需要设计一个线性函数来使这个误差随时间大幅度减小或者趋于0，这里先不细说了。同样最后也要注意该模块正常使用的工作电压，按照常理来说能使用与规定电压略低的电压，但是最好不要高于这个电压，这样不利于长期的使用。

 3.OLED模块

1.首先我想说的是学过51单片机的小伙伴应该知道在51开发板套件中有一个长方形的LED显示屏，而这里的OLED显示屏的原理和LED显示屏的原理其实相差不大，只是为了减少外部GPIO引脚使用过多，从而使用I2C通信来解决这个问题，实质上通信也是为了得到在哪个像素点需要点亮，最终就可以组成数字字母或者进行取模得到汉字或者其他图案。这里如果对其代码底层原理好奇的话，可以自行学习，为了减小篇幅就不细细解释了。

 二.对于各部件相联系来使小车直行，直角弯等的原理总结

1.就是核心的PID算法，这个算法其实听起来感觉很难，但是花一定时间去思考就会很容易理解，然后你就会很容易的理解平衡车的工作原理，还有四轴飞行器的原理，都是依靠这个算法实现的，下面我先将PID的官方原理写出来。

首先比如，我想控制一个“热得快”，让一锅水的温度保持在50℃，这么简单的任务，为啥要用到微积分的理论。此时你会想这不是so easy嘛~ 小于50度就让它加热，大于50度就断电，不就行了？几行代码用stm32分分钟写出来。没错~在要求不高的情况下，确实可以这么干,但如果换一种说法，你就知道问题出在哪里了。这里如果我要控制一辆汽车要是希望汽车的车速保持在50km/h不动，你还敢这样干么。设想一下，假如汽车的定速巡航电脑在某一时间测到车速是45km/h。它立刻命令发动机：加速！结果，发动机那边突然来了个100%全油门，嗡的一下，汽车急加速到了60km/h。这时电脑又发出命令：刹车！结果，吱...............哇............(乘客吐)所以，在大多数场合中，用“开关量”来控制一个物理量，就显得比较简单粗暴了。有时候，是无法保持稳定的。因为单片机、传感器不是无限快的，采集、控制需要时间。而且，控制对象具有惯性。比如你将一个加热器拔掉，它的“余热”（即热惯性）可能还会使水温继续升高一小会。

这样就引申出这个算法

PID=Uk+KP*【E(k)-E(k-1)】+KI*E(k)+KD*【E(k)-2E(k-1)+E(k-2)】

KP :此处的KP相当于是每次的实际值与预期值之间差(arr)的比例系数，每次该差值都要乘以这个系数，从而更容易改变轮子的PWM，也就是更好的改变速度，其中E(K)就是每次误差arr的累加值（arr2-arr1），相当于是对于下一时间变化的预测。比如，我用编码器测出这个轮的速度为500，

而我想要的速度为600，这里arr的值就为600-500=100，这样在乘KP这个系数加在这个轮的PWM上就会使这个轮的转速更接近600，这里只是理想化，其实现实中不可能这么精准的调节pwm使其速度到预期值。

KI: 而KI就是和KP差不多也是要乘的比例系数，便于更好的控制PWM。其中E(K)就是每次误差arr的累加值（arr2-arr1），相当于是对于下一时间变化的预测。比如我第一次得到的arr值为100，下次可能为50，这样累加就是150，乘KI这个系数就可以加在pwm上了，就又使速度更加接近预定速度。但是值得注意的是这个累加值不能无限的让它加，这样尽管数值可能很小但是经过一段时间数值就会非常大，而如果把这么大的值尽管乘以了系数KI也会很大，加在轮子的PWM上，会使转速超过预定值，所以，这里我们用一个if判断，如果值加的超过一定数值，就让这个累加值变成0，这样就可以了。

KD:  还有一个就是KD,也是一个系数，只不过这次乘以系数的是两次arr之间的值，为了让小车更快的调节到预定的速度，而不在预定速度上下跳动，或超出预定速度。

UK :相当于一个基础速度值，可以减少整个过程的时间，比如，要想速度到600，我们不能直接让轮子从速度为零开始吧，这样会浪费一定的时间。如果此时我从500的速度开始时不时经过很少时间的PID调节就可以达到预期速度600呢。

2.我这次可以说是使用的角度环PID，简单来说就是我每次的arr不是预期速度与实际速度的差值，而是预期角度与实际角度的差值，通过这个去调节pwm从而调节速度，其实原理是一样的。把我上面说的理解了，这里就会想通。但是这里要注意的一点是我前面说的需要得到由于mpu6050产生的随时间变化数据的漂移。简单来说就是小车不动，这个角度数值会一直加，所以此处需要一个随时间变化的线性回归函数，让mpu6050得到的数值减去这个函数值就可以了，我这里其实也是放在下面说的定时器中断中了，然后如果觉得定时中断时间太短，可以设定一个变量，比如，你的中断是100us进入一次，那我这个线性回归函数想以s为单位，那我设定一个变量counter,每进入一次中断就加1，那么当加10000次就是1s，这里直接if判断条件就可以了。

       所以这里就来说我怎么实现实时得到arr的值并且其累加值I呢，我是用了一个定时器里的定时中断，使得在一定时间内实时更新arr值来实现这样数据的变化，然后定义一个全局变量，将这个变量在main函数上前面加上extern  外部引用标识，将其赋给pwm就可以了。然后如果想接上OLED也可以，这样就可以实时看到向哪里偏离了几度。这里我建议的KP的调节范围是1~10，KI范围是10~60，KD调节范围是0~80，这里合适你的小车数值只能自己去实验得到了。

三.主体代码和mpu6050与OLED驱动代码

#include <stm32f10x.h>
 
extern    int pwm1,pwm2;//左右轮分别对应的pwm 
extern    int counterRight;//进中断计数控制右轮pwm 
extern    int counterLeft;//控制左轮pwm 
extern    int  i;//放在定时器中断中，每次加1相当于过了10us 
int       ms; 每此加1相当于一秒，用于我文章说的线性回归方程，减小角度误差 
extern   float   differenceYaw;//回归方程的返回值 
extern   float    arr;//预期值与实际值的差 
extern   float   prvious_yaw;//自己定义的预期值 
extern   float   Yaw;//mpu6050返回的带有误差的漂移值 
extern   int  m;//控制马达是向哪个方向走 
float    Itotal;//I的累加值 
float    P;//下同，都为算法的比例系数 
float    I;
float    D;
 
 
void  timer3_Init()
{
 
TIM_TimeBaseInitTypeDef     TIM_TimeBaseInitStruct;//定义各结构体 
GPIO_InitTypeDef        GPIO_InitStruct;
GPIO_InitTypeDef        GPIO_InitStruct1;
NVIC_InitTypeDef         NVIC_InitStruct;
 
	
RCC_APB1PeriphClockCmd(RCC_APB1Periph_TIM3,ENABLE);//开启定时器3的时钟 
 
TIM_InternalClockConfig(TIM3);//给定时器3配置时钟频率 
	
GPIO_InitStruct.GPIO_Mode=GPIO_Mode_Out_PP;//使能输出引脚，用于输出PWM，这里我是使用定时中断产生pwm的
                                            //相当于软件产生pwm，也可以采用stm32定时器硬件产生pwm. 
GPIO_InitStruct.GPIO_Pin=GPIO_Pin_0|GPIO_Pin_1|GPIO_Pin_2|GPIO_Pin_3;
GPIO_InitStruct.GPIO_Speed=GPIO_Speed_50MHz;
GPIO_Init(GPIOA,&GPIO_InitStruct);
	
 
 
TIM_TimeBaseInitStruct.TIM_ClockDivision=TIM_CKD_DIV1;//设置时钟分割 
TIM_TimeBaseInitStruct.TIM_CounterMode=TIM_CounterMode_Up;//向上计数 
TIM_TimeBaseInitStruct.TIM_Period=10-1;//计数 
TIM_TimeBaseInitStruct.TIM_Prescaler=72-1;//分频 
TIM_TimeBaseInitStruct.TIM_RepetitionCounter=0;
TIM_TimeBaseInit(TIM3,&TIM_TimeBaseInitStruct);
 
TIM_ClearFlag(TIM3, TIM_FLAG_Update); //清除中断标志位      
TIM_ITConfig(TIM3,TIM_IT_Update,ENABLE);//使能更新中断 
 
NVIC_PriorityGroupConfig(NVIC_PriorityGroup_2);//分组 
NVIC_InitStruct.NVIC_IRQChannel=TIM3_IRQn;//中断函数配置 
NVIC_InitStruct.NVIC_IRQChannelCmd=ENABLE;//打开该中断通道 
NVIC_InitStruct.NVIC_IRQChannelPreemptionPriority=2;//优先级的使用 
NVIC_InitStruct.NVIC_IRQChannelSubPriority=1;
NVIC_Init(&NVIC_InitStruct);	
 
 
 
TIM_Cmd(TIM3,ENABLE);//打开定时器三 
 
 
 
}
 
 
 
/*void  PID(float x,float y,float z)
{
P=x;
y=I;
z=D;
}*/
 
 
 
 
 
void   TIM3_IRQHandler()
{
 
   if(TIM_GetITStatus(TIM3,TIM_IT_Update)==SET)
	{
		i++;
		if(i==100000)
		{
		ms++;
		differenceYaw=ms*0.04-0.489;
			i%=100000;
		}
		counterLeft++;
		counterRight++;
		counterLeft%=1001;
		counterRight%=1001;
		
	
	arr=(Yaw-6.9-differenceYaw)-prvious_yaw;
		
	Itotal+=arr;
		if(Itotal>=200)
		{
		
		Itotal=0;
		}
		
		
		if(Itotal<=-D+100)
		{
		
		Itotal=0;
		
		}
  pwm1=P*arr+I*Itotal+D;;	
		
		if(pwm1>=D+300)
		{
		pwm1=D+150;
		
		}
		
		if(pwm1<=0)
		{
		
		
		pwm1=10;
		}
		
		*/
		
	
 
		if(m==1)//直行
		{
		
		if(counterLeft<=pwm1)
		{
		GPIO_SetBits(GPIOA,GPIO_Pin_0);
		GPIO_ResetBits(GPIOA,GPIO_Pin_1);
		
		}
		
		else
		{
		
		GPIO_ResetBits(GPIOA,GPIO_Pin_0);
		GPIO_ResetBits(GPIOA,GPIO_Pin_1);
		
		
		
		}
		
		if(counterRight<=pwm2)
		{
		
		GPIO_SetBits(GPIOA,GPIO_Pin_2);
		GPIO_ResetBits(GPIOA,GPIO_Pin_3);
	
		}
		
		
	else
	{
	
	GPIO_ResetBits(GPIOA,GPIO_Pin_2);
	GPIO_ResetBits(GPIOA,GPIO_Pin_3);
	
	
	}
		
		
		
		
	}
		
		if(m==2)//向后
		{
		
		 if(counterLeft<=pwm1)//后退
		 {
		 
		 
		GPIO_ResetBits(GPIOA,GPIO_Pin_0);
		GPIO_SetBits(GPIOA,GPIO_Pin_1);
		 
		 
		 
		 }
	  
        else
		{
		
		GPIO_ResetBits(GPIOA,GPIO_Pin_0);
		GPIO_ResetBits(GPIOA,GPIO_Pin_1);
		
		}
		
		if(counterRight<=pwm2)
		{
		
		
		GPIO_ResetBits(GPIOA,GPIO_Pin_2);
		GPIO_SetBits(GPIOA,GPIO_Pin_3);
			
		}
		
		else
		{
		
		GPIO_ResetBits(GPIOA,GPIO_Pin_2);
		GPIO_ResetBits(GPIOA,GPIO_Pin_3);
		
		
		}
		
	}
		
	
	  if(m==3)
		{
		
		if(counterLeft<=pwm1)//左转
		 {
		 
		 
		GPIO_ResetBits(GPIOA,GPIO_Pin_0);
		GPIO_SetBits(GPIOA,GPIO_Pin_1);
		 
		 
		 
		 }
	  
        else
		{
		
		GPIO_ResetBits(GPIOA,GPIO_Pin_0);
		GPIO_ResetBits(GPIOA,GPIO_Pin_1);
		
		}
		
		if(counterRight<=pwm2)
		{
		
		GPIO_SetBits(GPIOA,GPIO_Pin_2);
		GPIO_ResetBits(GPIOA,GPIO_Pin_3);
		
		}
		
		else
		{
		
		GPIO_ResetBits(GPIOA,GPIO_Pin_2);
		GPIO_ResetBits(GPIOA,GPIO_Pin_3);
		
		
		}
		
		
		
		
		
		
		
		}
	
	
	
	
	if(m==4)
	{
	
		
		if(counterLeft<=pwm1)//右转
		 {
		 
		 
		
		GPIO_SetBits(GPIOA,GPIO_Pin_0);
		GPIO_ResetBits(GPIOA,GPIO_Pin_1);
		 
		 
		 }
	  
        else
		{
		
		GPIO_ResetBits(GPIOA,GPIO_Pin_0);
		GPIO_ResetBits(GPIOA,GPIO_Pin_1);
		
		}
		
		if(counterRight<=pwm2)
		{
		
		GPIO_ResetBits(GPIOA,GPIO_Pin_2);
		GPIO_SetBits(GPIOA,GPIO_Pin_3);
			
		}
		
		else
		{
		
		GPIO_ResetBits(GPIOA,GPIO_Pin_2);
		GPIO_ResetBits(GPIOA,GPIO_Pin_3);
		
		
		}
		
		
		
	}	
	
	
	if(m==5)
	{
	
	GPIO_ResetBits(GPIOA,GPIO_Pin_0);
	GPIO_ResetBits(GPIOA,GPIO_Pin_1);
	GPIO_ResetBits(GPIOA,GPIO_Pin_2);
	GPIO_ResetBits(GPIOA,GPIO_Pin_3);
	
	
	}
	
	
	
	
		
TIM_ClearITPendingBit(TIM3,TIM_IT_Update);
		
}		
		
	}	
		
		
		

这里的extern 是由于我在主函数定义了全局变量，这里是跨越.c文件外部使用需要用extern 标识符声明变量，主函数没什么内容，只是while循环调用OLED显示，这里就不发出来了。

                                   以下是两个模块的驱动代码的百度云盘链接

                                   链接最好复制链接到浏览器打开要不有些情况下打不开~

链接：https://pan.baidu.com/s/1q7wbvX9Ika4vdVFGxNc8VQ?pwd=0508 提取码：0508