系列文章目录

编码电机测速

HAL库OLED的使用

HAL库版STM32双轮自平衡车(一) ———代码思路和PID基础精讲

HAL库版STM32双轮自平衡车(二) ——— CubeMX的配置、原理图接线、物料准备

HAL库版STM32双轮自平衡车(三) ———代码精讲

HAL库版STM32双轮自平衡车(四) ———— 原理图以及PCB绘制

HAL库版STM32双轮自平衡车(五) ———— 调参

完整工程——>闲鱼搜店铺  “黄金独角兽的小店”

完整工程链接（设置了收费望支持理解）https://h5.m.goofish.com/item?id=681989715310&ut_sk=1.X2hUgNjQh0QDAMrgZaxtwchr_21407387_1684499716074.copy.detail.681989715310.2209260974794&forceFlush=1&ownerId=9db66f5694ab477fc49456657355a970&un=b92a505ad4316dfe46071105c2583afc&share_crt_v=1&un_site=77&spm=a2159r.13376460.0.0&sp_abtk=common_xianyu_commonInfo&sp_tk=UndhbWRvMlUwNzc%3D&cpp=1&shareurl=true&short_name=h.UEhbs71&bxsign=scd2vrEz8ec8281p2gBEWXb_hK1esb_MFtgLX0U9fHSxWHWEUxw2KDwuHFLf7dA0QUH1VGbvJXD3BKyKhMRbpxqeMktn1Tlm-MUD_wJhhrmkLdpYFWkc1t0KApMVQN8nj5NZvwrV_KDRe53CoD_5Yi2wg&tk=Rwamdo2U077&app=chrome

目录

系列文章目录

前言

核心代码

一、PID控制部分

1.1 直立环PD控制

1.2 速度环PI控制

二、电机控制代码

三、编码器测速代码

四、mpu6050的INT外部中断代码

五、MPU6050部分代码

六、main.c 代码

总结

前言

上一篇文章重点讲解了CubeMX的配置、原理图接线、物料准备，那么接下来本文将一步步开始编写小车的代码，精讲其中的代码原理。下一篇文章是 原理图以及PCB的绘制。

代码的编写环境是Keil5，当然我也有CubeIDE的版本，这里仅展示Keil的，这篇文章将平衡小车的核心以及绝大多数代码已经开源，如果有兄台想要完整工程以及我总结的学习资料可以到文章开头链接获取。

文章中有涉及到OLED显示以及直流减速电机的测速，这两部分不再赘述，想要了解的兄台请移步至我的主页或是点击本文一开始的系列文章目录学习。

核心代码

一、PID控制部分

首先再次重复一遍P,I,D这三个控制器的作用和缺点：

p控制器（比例）

作用是：减小测量值和期望值之间的误差，让测量值不断接近期望值。

缺点是：存在稳态误差（静差）。若存在固定扰动时，P控制律不能迅速响应。

I控制器（积分）

作用是：消除静差，所谓静差，就是系统稳定后输出值和设定值之间的差值，积分环节实际上就是偏差累计的过程，把累计的误差加到原有系统上以抵消系统造成的静差。

缺点是：控制不及时。因为积分输出的累积是渐进的，其产生的控制作用总是落后于偏差的变化,不能及时有效地克服干扰的影响，难以使控制系统稳定下来。

D控制器（微分）

作用是：反应了偏差信号的变化规律，或者说是变化趋势，根据偏差信号的变化趋势来进行超前调节，从而增加了系统的快速性。

缺点是：放大噪声。

1.1 直立环PD控制

直立环部分用了PD控制，即没用用到 I（积分）控制

  • 那么就有疑问了，为什么用 PD 而不用 PI 呢？

当角度接近机械零度时，P控制器就不工作了，控制器认为完成了任务，然而控制器忽略了角度在机械零度时电机还存在角速度，简单来说就是小车还有惯性。这时候就需要加入D控制器，预测下一时刻的偏差，进行超前调节，从而增加了系统的快速性。反过来想就是这个PID控制器中为什么不用积分。这里必须知道的一点是当比例系数很大的时候系统几乎没有静差。平衡小车直立控制需要快速性，相比之下P是很大的，积分的作用是消除静差，而静差对于直立控制的影响很小自然就不需要积分了。实际上平衡小车不一定要完全处于机械中值，稍微偏一点点也没有关系，只要稳定就行了。再有一点，小车需要尽可能快的达到直立平衡，I控制存在着控制不及时的缺点。

所以对于准确性要求不高的系统可以不加入积分控制。
 

/**
 * @function: int Up_balance(float Angle,float Gyro,float Mechanical_Angle)
 * @description:直立环PD控制
 * @param {float} Angle 数据 :mpu6050测量的俯仰角
 * @param {float} Gyro 数据 :mpu6050测量的沿y轴加速度
 * @param {float} Mechanical_Angle 数据 :机械平衡角度（机械中值),这里设置为0
 * @return {int} Up_balance :直立环控制PWM
 */
int Up_balance(float Angle,float Gyro,float Mechanical_Angle)
{
	float Bias; //角度误差值
	int balance_up; //直立环控制PWM
	Bias=Angle-Mechanical_Angle; //角度误差值==测量的俯仰角-理想角度（机械平衡角度）
	balance_up=Up_balance_KP*Bias+Up_balance_KD*Gyro; //计算平衡控制的电机PWM  PD控制   Up_balance_KP是P系数,Up_balance_KD是D系数
	return balance_up;
}

直立环控制的代码十分容易理解，代码注释也清晰明了，如果对位置式PID不太清晰可以看看系列文章的第一篇，我这里就不多说了。 

1.2 速度环PI控制

速度环部分用了PI控制，即没用用到 D（微分）控制

  • 平衡小车直立为什么还需要速度环？

小车光有直立环也是可以直立一小会儿的，但是一但外界有干扰（比如用手推了小车），小车只能通过单方向运动维持平衡，加入速度环就是修正小车的角度，小车一直往前面开时，速度环让小车以更快速度行驶使得小车后仰，这时直立控制让小车后退以维持平衡。

  • 还是老问题，为什么用 PI 而不用 PD 呢？

对于速度控制当然是越精确越好，平衡小车当然希望速度一直为0。但是光有P控制器是不够的，因为P控制器的主要缺点是存在静差。那么就需要用I积分控制器来消除静差。

虽然D控制器可以预测未来，提前响应，但是微分控制的缺点是放大噪声。速度控制的速度偏差是由直立控制电机运动引起的，即速度噪声很大，所以不加入微分控制。

所以对于准确性要求高，系统噪声大时可以采用PI控制。

/**
 * @function:int Velocity(int Encoder_left,int Encoder_right,int Mechanical_velocity)
 * @description:速度环PI控制
 * @param {int} Encoder_left 数据 :左电机编码器的值
 * @param {int} Encoder_right 数据 :右电机编码器的值
 * @param {int} Mechanical_velocity 数据 :目标速度 ，因为只是让小车尽快平衡并静止，这里目标速度也设为0
 * @return {int} Up_balance :速度环控制PWM
 */
int Velocity(int Encoder_A,int Encoder_B,int Mechanical_velocity)
{
	int velocity,Encoder_Least; //速度环控制PWM,获取最新速度偏差
	static float Encoder,Encoder_Integral; //一阶低通滤波后的速度值，速度的积分；因为积分累加和滤波的需要，故设置为静态量，存储在全局区域类似于全局变量

	Encoder_Least =(Encoder_A+Encoder_B)-Mechanical_velocity; //获取最新速度偏差==测量速度（左右编码器之和）-目标速度                                            
	Encoder = Encoder * 0.8+Encoder_Least*0.2; //一阶低通滤波器,减小速度环对于直立环的负面影响
	Encoder_Integral +=Encoder;               //积分，就是累加
	Encoder_Integral =Encoder_Integral>10000?10000:(Encoder_Integral<-10000?-10000:Encoder_Integral); //积分限幅
	velocity=Velocity_KP*Encoder+Velocity_KI*Encoder_Integral;   //速度控制 PI控制   Velocity_KP是P系数,Velocity_KI是I系数
	if(pitch<-30||pitch>30) 			Encoder_Integral=0;   //电机关闭后清除积分
	return velocity;
}

这里重点说一下速度环代码中的一阶低通滤波器，要小车直立起来并且达到平衡，我直接使用叠加式的并行PID，所以需要对速度控制进行低通滤波，即削弱速度控制的效果，让平衡控制占主导地位。

下面给出一阶低通滤波公式：

Y（n）=αX（n） + （1-α）Y（n-1）

式中：α=滤波系数；X（n）=本次采样值；Y（n-1）=上次滤波输出值；Y（n）=本次滤波输出值。

二、电机控制代码

/**
 * @function: void Give_Motor_PWM(int MotorL_PWM,int MotorR_PWM)
 * @description: 赋值PWM，控制电机正反转
 * @param {int} MotorL_PWM 数据 :左电机PWM值
 * @param {int}  MotorR_PWM数据 :右电机PWM值
 * @return {*}
 */
void Give_Motor_PWM(int MotorL_PWM,int MotorR_PWM)
{
	if (MotorL_PWM>0) //左电机正转
	{
		HAL_GPIO_WritePin(AIN1_GPIO_Port, AIN1_Pin, GPIO_PIN_SET);
		HAL_GPIO_WritePin(AIN2_GPIO_Port, AIN2_Pin, GPIO_PIN_RESET);
	}
	else              //左电机反转
	{
		HAL_GPIO_WritePin(AIN1_GPIO_Port, AIN1_Pin, GPIO_PIN_RESET);
		HAL_GPIO_WritePin(AIN2_GPIO_Port, AIN2_Pin, GPIO_PIN_SET);
	}
	if (MotorR_PWM>0) //右电机正转
	{
		HAL_GPIO_WritePin(BIN1_GPIO_Port, BIN1_Pin, GPIO_PIN_SET);
		HAL_GPIO_WritePin(BIN2_GPIO_Port, BIN2_Pin, GPIO_PIN_RESET);
	}
	else              //右电机反转
	{
		HAL_GPIO_WritePin(BIN1_GPIO_Port, BIN1_Pin, GPIO_PIN_RESET);
		HAL_GPIO_WritePin(BIN2_GPIO_Port, BIN2_Pin, GPIO_PIN_SET);
	}

	__HAL_TIM_SET_COMPARE(&htim2,TIM_CHANNEL_1, ABS(MotorL_PWM)+180);//180为死区电压对应的值
	__HAL_TIM_SET_COMPARE(&htim2,TIM_CHANNEL_2, ABS(MotorR_PWM)+200);
}

/**
 * @function: void Limit(int *PWMA,int *PWMB)
 * @description: PWM限幅函数:避免PWM过大超过马达的机械极限,增加电机寿命
 * @param {int*} *PWMA 数据 :左电机PWM值
 * @param {int*} *PWMB 数据 :右电机PWM值
 * @return {*}
 */
void Limit(int *PWMA,int *PWMB)
{
	*PWMA=*PWMA>6900?6900:(*PWMA<-6900?-6900:*PWMA);
	*PWMB=*PWMB>6900?6900:(*PWMB<-6900?-6900:*PWMB);
}

**
 * @function: int ABS(int a)
 * @description: 绝对值函数
 * @param {int} a 数据 :输入的值
 * @return {int} a :取绝对值输出
 */
int ABS(int a)
{
	a = a>0?a:(-a);
	return a;
}

上述代码第一个是电机驱动模块的代码，根据TB6612，DRV8848的真值表编写（都类似于L298N），代码中最后设置PWM占空比时添加了死区电压对应值，这个可以根据实际测量得出，初始将PID得出的PWM值设置为0，再将死区电压对应值从0开始慢慢增大直至轮子开始转动即可。

三、编码器测速代码

/**
 * @function: void Get_Encoder_Counter(int *Encoder_Left_Counter,int *Encoder_Right_Counter)
 * @description: 获取编码器计数值
 * @param {int*} *Encoder_Left_Counter 数据 :左电机编码器的值
 * @param {int*} *Encoder_Right_Counter 数据 :右电机编码器的值
 * @return {*}
 */
void Get_Encoder_Counter(int *Encoder_Left_Counter,int *Encoder_Right_Counter)
{
	*Encoder_Left_Counter =(short) __HAL_TIM_GET_COUNTER(&htim3);     //保存编码器计数器的值
	*Encoder_Right_Counter =(short) __HAL_TIM_GET_COUNTER(&htim4);
	__HAL_TIM_SET_COUNTER(&htim3,0);      //保存之后要清零,以便下次继续读取.另外每次清零后采样值减0,直接用单位时间的话就可以得出速度信息了，不用麻烦还要减去初值了
	__HAL_TIM_SET_COUNTER(&htim4,0);
}

编码器测速部分我在另一篇博客详细介绍过，想要了解的兄台请移步至我的主页或是点击本  文一开始的系列文章目录学习。

四、mpu6050的INT外部中断代码

void HAL_GPIO_EXTI_Callback(uint16_t GPIO_Pin)//在inv_mpu.h设置的mpu6050采样率为100Hz,即10ms进入一次外部中断
{
	switch(GPIO_Pin)
	{
		case INT_Pin :

		   MPU_DMP_Get_Data(&pitch,&roll,&yaw);//得到俯仰角pitch
		   MPU_Get_Gyroscope(&gx,&gy,&gz);    //得到y轴加速度值
		   Get_Encoder_Counter(&Encoder_Left,&Encoder_Right); //得到左右编码器计数值即左右电机转速
		   balance_up=Up_balance(pitch,gy,Mechanical_Angle); //直立环
		   velocity=Velocity( Encoder_Left,Encoder_Right,Mechanical_velocity); //速度环
		   PWMA= balance_up+velocity; //并联直立环与速度环
		   PWMB= balance_up+velocity; 
		   Limit(&PWMA,&PWMB);        //PWM限幅
		   if(pitch<-30||pitch>30)    //检查俯仰角的角度值，这里增加这个判断的目的是：当小车失去平衡后立即关闭电机
		   {
			   PWMA=0;
			   PWMB=0;
		   }
		   Give_Motor_PWM(PWMA,PWMB); //赋值给PWM寄存器以及控制电机正反转

//		 printf("pitch:%f\t,roll:%f\t,yaw:%f\r\n",pitch,roll,yaw);测试用的

			break;
		default:
			break;
  }
}

在inv_mpu.h设置的mpu6050采样率为100Hz,即10ms进入一次外部中断

五、MPU6050部分代码

mpu6050这部分代码就是移植正点原子的，另外添加了一些自己的整定代码。由于篇幅的限制，这里就不把所有的驱动代码列在这里了。

#include "mpu6050.h" 
#include "stdio.h"

//陀螺仪方向设置
static signed char gyro_orientation[9] = { 1, 0, 0,
                                           0, 1, 0,
                                           0, 0, 1};

//初始化MPU6050
//返回值:0,成功
//    其他,错误代码
uint8_t MPU_Init(void)
{ 
  uint8_t res;
//	printf("\r\nReady to init MPU6050\r\n");
  res=MPU_Read_Byte(MPU_DEVICE_ID_REG);
  if(res==MPU_ADDR)//器件ID正确
  {
//		printf("mpu6050 ID Read: OK at 0x68\r\n");
		MPU_Write_Byte(MPU_PWR_MGMT1_REG,0X80);	//复位MPU6050
		MPU_Write_Byte(MPU_PWR_MGMT1_REG,0X00);	//唤醒MPU6050
		MPU_Set_Gyro_Fsr(3);					//陀螺仪传感器,±2000dps
		MPU_Set_Accel_Fsr(0);					//加速度传感器,±2g
		MPU_Set_Rate(50);						//设置采样率50Hz

		MPU_Write_Byte(MPU_INT_EN_REG,0X00);	//关闭所有中断
		MPU_Write_Byte(MPU_USER_CTRL_REG,0X00);	//I2C主模式关闭
//		MPU_Write_Byte(MPU_FIFO_EN_REG,0X78);	//=====使能FIFO
		MPU_Write_Byte(MPU_FIFO_EN_REG,0X00);	//=====使能FIFO


//		MPU_Write_Byte(MPU_INTBP_CFG_REG,0X9C);	//=====INT引脚中断信号高电平有效
//	   MPU_Write_Byte(MPU_INT_EN_REG	,0X01);	//=====INT中断使能
	   MPU_Write_Byte(MPU_INTBP_CFG_REG,0X80);	//INT引脚静息为高电平，中断信号低电平有效
     MPU_Write_Byte(MPU_PWR_MGMT1_REG,0X01);	//设置CLKSEL,PLL X轴为参考
     MPU_Write_Byte(MPU_PWR_MGMT2_REG,0X00);	//加速度与陀螺仪都工作
     MPU_Set_Rate(50);						//=====设置采样率为50Hz
		return 0;
  }
	else
	{
		printf("Err mpu id:0x%x\r\n", res);
		return 1;
	}
}

//MPU6050INT中断初始化
void MPU_INT_Init(void)
{
//	MPU_Write_Byte(MPU_INTBP_CFG_REG,0X9C);	//=====INT引脚中断信号高电平有效
	MPU_Write_Byte(MPU_INT_EN_REG	,0X01);	//=====INT中断使能
	MPU_Write_Byte(MPU_INTBP_CFG_REG,0X80);	//=====INT引脚中断信号电平低有效
}


//MPU6050自测试
//返回值:0,正常
//    其他,失败
uint8_t Run_Self_Test(void)
{
	int result;
	//char test_packet[4] = {0};
	long gyro[3], accel[3];
	result = mpu_run_self_test(gyro, accel);
	if (result == 0x7)
	{
		/* Test passed. We can trust the gyro data here, so let's push it down
		* to the DMP.
		*/
		float sens;
		unsigned short accel_sens;
		mpu_get_gyro_sens(&sens);
		gyro[0] = (long)(gyro[0] * sens);
		gyro[1] = (long)(gyro[1] * sens);
		gyro[2] = (long)(gyro[2] * sens);
		dmp_set_gyro_bias(gyro);
		mpu_get_accel_sens(&accel_sens);
		accel[0] *= accel_sens;
		accel[1] *= accel_sens;
		accel[2] *= accel_sens;
		dmp_set_accel_bias(accel);
		return 0;
	}else return 1;
}
//陀螺仪方向控制
unsigned short inv_orientation_matrix_to_scalar(
    const signed char *mtx)
{
    unsigned short scalar;
    /*
       XYZ  010_001_000 Identity Matrix
       XZY  001_010_000
       YXZ  010_000_001
       YZX  000_010_001
       ZXY  001_000_010
       ZYX  000_001_010
     */

    scalar = inv_row_2_scale(mtx);
    scalar |= inv_row_2_scale(mtx + 3) << 3;
    scalar |= inv_row_2_scale(mtx + 6) << 6;


    return scalar;
}
//方向转换
unsigned short inv_row_2_scale(const signed char *row)
{
    unsigned short b;

    if (row[0] > 0)
        b = 0;
    else if (row[0] < 0)
        b = 4;
    else if (row[1] > 0)
        b = 1;
    else if (row[1] < 0)
        b = 5;
    else if (row[2] > 0)
        b = 2;
    else if (row[2] < 0)
        b = 6;
    else
        b = 7;      // error
    return b;
}
//空函数,未用到.
void mget_ms(unsigned long *time)
{

}
//mpu6050,dmp初始化
//返回值:0,正常
//    其他,失败
uint8_t MPU_DMP_Init(void)
{
	uint8_t res=0;
	if(mpu_init()==0)	//初始化MPU6050
	{
		res=mpu_set_sensors(INV_XYZ_GYRO|INV_XYZ_ACCEL);//设置所需要的传感器
		if(res)return 1;
//		HAL_Delay(10);
		res=mpu_configure_fifo(INV_XYZ_GYRO|INV_XYZ_ACCEL);//设置FIFO
		if(res)return 2;
		res=mpu_set_sample_rate(DEFAULT_MPU_HZ);	//设置采样率
		if(res)return 3;
		res=dmp_load_motion_driver_firmware();		//加载dmp固件
		if(res)return 4;
		res=dmp_set_orientation(inv_orientation_matrix_to_scalar(gyro_orientation));//设置陀螺仪方向
		if(res)return 5;
		res=dmp_enable_feature(DMP_FEATURE_6X_LP_QUAT|DMP_FEATURE_TAP|	//设置dmp功能
		    DMP_FEATURE_ANDROID_ORIENT|DMP_FEATURE_SEND_RAW_ACCEL|DMP_FEATURE_SEND_CAL_GYRO|
		    DMP_FEATURE_GYRO_CAL);
		if(res)return 6;
		res=dmp_set_fifo_rate(DEFAULT_MPU_HZ);	//设置DMP输出速率(最大不超过200Hz)
		if(res)return 7;
		res=Run_Self_Test();		//自检
//		if(res)return 8;
		res=mpu_set_dmp_state(1);	//使能DMP
		if(res)return 9;
	}else return 10;
	MPU_INT_Init();//=====
	return 0;
}
//MPU6050初始化
void MPU6050_Init(void)
{
	MPU_Init();//=====初始化MPU6050
  HAL_Delay(100);
  while(MPU_DMP_Init())//=====初始化MPU6050的DMP模式
	{
		HAL_Delay(20);
	}
//  printf("-- DMP Init OK-- \r\n");
}

//得到温度值
//返回值:温度值(扩大了100倍)
float MPU_Get_Temperature(void)
{
  unsigned char  buf[2]; 
  short raw;
  float temp;
  
  MPU_Read_Len(MPU_TEMP_OUTH_REG,2,buf); 
  raw=(buf[0]<<8)| buf[1];  
  temp=(36.53+((double)raw)/340)*100;  
//  temp = (long)((35 + (raw / 340)) * 65536L);
  return temp/100.0f;
}
//得到陀螺仪值(原始值)
//gx,gy,gz:陀螺仪x,y,z轴的原始读数(带符号)
//返回值:0,成功
//    其他,错误代码
uint8_t MPU_Get_Gyroscope(short *gx,short *gy,short *gz)
{
  uint8_t buf[6],res;  
	res=MPU_Read_Len(MPU_GYRO_XOUTH_REG,6,buf);
	if(res==0)
	{
		*gx=((uint16_t)buf[0]<<8)|buf[1];  
		*gy=((uint16_t)buf[2]<<8)|buf[3];  
		*gz=((uint16_t)buf[4]<<8)|buf[5];
	} 	
    return res;
}
//得到加速度值(原始值)
//ax,ay,az:陀螺仪x,y,z轴的原始读数(带符号)
//返回值:0,成功
//    其他,错误代码
uint8_t MPU_Get_Accelerometer(short *ax,short *ay,short *az)
{
    uint8_t buf[6],res;  
	res=MPU_Read_Len(MPU_ACCEL_XOUTH_REG,6,buf);
	if(res==0)
	{
		*ax=((uint16_t)buf[0]<<8)|buf[1];  
		*ay=((uint16_t)buf[2]<<8)|buf[3];  
		*az=((uint16_t)buf[4]<<8)|buf[5];
	} 	
    return res;;
}

//得到dmp处理后的数据(注意,本函数需要比较多堆栈,局部变量有点多)
//pitch:俯仰角 精度:0.1°   范围:-90.0° <---> +90.0°
//roll:横滚角  精度:0.1°   范围:-180.0°<---> +180.0°
//yaw:航向角   精度:0.1°   范围:-180.0°<---> +180.0°
//返回值:0,正常
//    其他,失败
uint8_t MPU_DMP_Get_Data(float *pitch,float *roll,float *yaw)
{
	float q0=1.0f,q1=0.0f,q2=0.0f,q3=0.0f;
	unsigned long sensor_timestamp;
	short gyro[3], accel[3], sensors;
	unsigned char more;
	long quat[4];
	if(dmp_read_fifo(gyro, accel, quat, &sensor_timestamp, &sensors,&more))return 1;
	/* Gyro and accel data are written to the FIFO by the DMP in chip frame and hardware units.
	 * This behavior is convenient because it keeps the gyro and accel outputs of dmp_read_fifo and mpu_read_fifo consistent.
	**/
	/*if (sensors & INV_XYZ_GYRO )
	send_packet(PACKET_TYPE_GYRO, gyro);
	if (sensors & INV_XYZ_ACCEL)
	send_packet(PACKET_TYPE_ACCEL, accel); */
	/* Unlike gyro and accel, quaternions are written to the FIFO in the body frame, q30.
	 * The orientation is set by the scalar passed to dmp_set_orientation during initialization.
	**/
	if(sensors&INV_WXYZ_QUAT)
	{
		q0 = quat[0] / q30;	//q30格式转换为浮点数
		q1 = quat[1] / q30;
		q2 = quat[2] / q30;
		q3 = quat[3] / q30;
		//计算得到俯仰角/横滚角/航向角
		*pitch = asin(-2 * q1 * q3 + 2 * q0* q2)* 57.3;	// pitch
		*roll  = atan2(2 * q2 * q3 + 2 * q0 * q1, -2 * q1 * q1 - 2 * q2* q2 + 1)* 57.3;	// roll
		*yaw   = atan2(2*(q1*q2 + q0*q3),q0*q0+q1*q1-q2*q2-q3*q3) * 57.3;	//yaw
	}else return 2;
	return 0;
}

//设置MPU6050陀螺仪传感器满量程范围
//fsr:0,±250dps;1,±500dps;2,±1000dps;3,±2000dps
//返回值:0,设置成功
//    其他,设置失败
uint8_t MPU_Set_Gyro_Fsr(uint8_t fsr)
{
	return MPU_Write_Byte(MPU_GYRO_CFG_REG,fsr<<3);//设置陀螺仪满量程范围
}
//设置MPU6050加速度传感器满量程范围
//fsr:0,±2g;1,±4g;2,±8g;3,±16g
//返回值:0,设置成功
//    其他,设置失败
uint8_t MPU_Set_Accel_Fsr(uint8_t fsr)
{
	return MPU_Write_Byte(MPU_ACCEL_CFG_REG,fsr<<3);//设置加速度传感器满量程范围
}
//设置MPU6050的数字低通滤波器
//lpf:数字低通滤波频率(Hz)
//返回值:0,设置成功
//    其他,设置失败
uint8_t MPU_Set_LPF(uint16_t lpf)
{
	uint8_t data=0;
	if(lpf>=188)data=1;
	else if(lpf>=98)data=2;
	else if(lpf>=42)data=3;
	else if(lpf>=20)data=4;
	else if(lpf>=10)data=5;
	else data=6;
	return MPU_Write_Byte(MPU_CFG_REG,data);//设置数字低通滤波器
}
//设置MPU6050的采样率(假定Fs=1KHz)
//rate:4~1000(Hz)
//返回值:0,设置成功
//    其他,设置失败
uint8_t MPU_Set_Rate(uint16_t rate)
{
	uint8_t data;
	if(rate>1000)rate=1000;
	if(rate<4)rate=4;
	data=1000/rate-1;
	data=MPU_Write_Byte(MPU_SAMPLE_RATE_REG,data);	//设置数字低通滤波器
 	return MPU_Set_LPF(rate/2);	//自动设置LPF为采样率的一半
}




//IIC连续写
uint8_t MPU_Write_Len(uint8_t reg,uint8_t len,uint8_t *buf)
{
  HAL_I2C_Mem_Write(&hi2c1, MPU_WRITE, reg, I2C_MEMADD_SIZE_8BIT, buf, len, 0xfff);
  HAL_Delay(1);
  
  return 0;
}
//IIC连续读
//addr:器件地址
//reg:要读取的寄存器地址
//len:要读取的长度
//buf:读取到的数据存储区
//返回值:0,正常
//    其他,错误代码
uint8_t MPU_Read_Len(uint8_t reg,uint8_t len,uint8_t *buf)
{ 
  HAL_I2C_Mem_Read(&hi2c1, MPU_READ, reg, I2C_MEMADD_SIZE_8BIT, buf, len, 0xfff);
  HAL_Delay(1);
  
  return 0;	
}
//IIC写一个字节
//reg:寄存器地址
//data:数据
//返回值:0,正常
//    其他,错误代码
uint8_t MPU_Write_Byte(uint8_t reg,uint8_t data) 				 
{ 
  unsigned char W_Data=0;

  W_Data = data;
  HAL_I2C_Mem_Write(&hi2c1, MPU_WRITE, reg, I2C_MEMADD_SIZE_8BIT, &W_Data, 1, 0xfff);
  HAL_Delay(1);
  
  return 0;
}
//IIC读一个字节
//reg:寄存器地址
//返回值:读到的数据
uint8_t MPU_Read_Byte(uint8_t reg)
{
  unsigned char R_Data=0;
  
  HAL_I2C_Mem_Read(&hi2c1, MPU_READ, reg, I2C_MEMADD_SIZE_8BIT, &R_Data, 1, 0xfff);
  HAL_Delay(1);
  
  return R_Data;		
}

六、main.c 代码

头文件这类的我就不多说了，毕竟每个人的想法都不太一样。

/* USER CODE BEGIN PV */
float Up_balance_KP=200;   //小车直立环P参数216
float Up_balance_KD=0;  //小车直立环D参数2
float Velocity_KP=0;   //小车速度环P参数-50
float Velocity_KI=0;    // 小车速度环I参数-0.25

float Mechanical_Angle=0;   //角度中值
int Mechanical_velocity=0;   //速度中值
int Encoder_Left,Encoder_Right=0;   //左右编码器的脉冲计数
int balance_up=0;
int velocity=0;
short gx,gy,gz=0;   //角速度
float pitch,roll,yaw=0;   //欧拉角
int PWMA,PWMB=0;   //计算出来的最终赋给电机的PWM

/* USER CODE END PV */
  /* USER CODE BEGIN 2 */
	
  HAL_NVIC_DisableIRQ(EXTI15_10_IRQn); //在NVIC中断控制器中关闭EXTI12中断，若不关闭就会影响mpu6050的初始化进程

  MPU6050_Init(); //、初始化mpu6050

  HAL_TIM_PWM_Start(&htim2,TIM_CHANNEL_1); //开启TIM2的PWM
  HAL_TIM_PWM_Start(&htim2,TIM_CHANNEL_2);
  HAL_TIM_Encoder_Start(&htim3, TIM_CHANNEL_ALL); //开启TIM3 4的编码器模式
  HAL_TIM_Encoder_Start(&htim4, TIM_CHANNEL_ALL);

  OLED_Init(); //OLED的初始化
  OLED_Clear();//OLED清屏
  OLED_ShowString(0,1,"pitch:",12,0);
  OLED_ShowString(0,3,"gy:",12,0);
  OLED_ShowString(0,5,"left:",12,0);
  OLED_ShowString(0,7,"right:",12,0);

  HAL_NVIC_EnableIRQ(EXTI15_10_IRQn);//在NVIC中断控制器中启用EXTI12中断
  /* USER CODE END 2 */

下面while (1)中的代码根据自己需求来写，也可以不添加OLED显示，我添加OLED的作用是方便看小车角度的机械中值，不过 角度的机械中值 也可以利用串口printf看。

  /* USER CODE BEGIN 3 */
		
	  OLED_Showdecimal(36,1,pitch,5,2,12, 0); //OLED显示小数值
	  OLED_Showdecimal(36,3,gy,5,2,12, 0);
	  OLED_Showdecimal(36,5,Encoder_Left,5,2,12, 0);
	  OLED_Showdecimal(36,7,Encoder_Right,5,2,12, 0);

  }
  /* USER CODE END 3 */

参考：

http://t.csdn.cn/9tC5w

总结

以上就是系列文章第三篇要讲的内容，本文讲解了怎么编写代码以及分析了代码的原理，之后也就是下一篇文章重点介绍在立创EDA中绘制原理图以及PCB。

完整工程和学习资料如下（博主也想尝尝秋天的第一杯奶茶，故设置了一瓶水的价格，望理解）

完整工程——>小黄鱼搜店铺  “黄金独角兽的小店”

码字不易，希望喜欢的小伙伴别忘了点赞+收藏+关注，你们的肯定就是我创作的动力


欢迎大家积极交流，本文未经允许谢绝转载！！！

# stm32 # 单片机 # arm
Logo
开放原子开发者工作坊

开放原子开发者工作坊旨在鼓励更多人参与开源活动，与志同道合的开发者们相互交流开发经验、分享开发心得、获取前沿技术趋势。工作坊有多种形式的开发者活动，如meetup、训练营等，主打技术交流，干货满满，真诚地邀请各位开发者共同参与！

更多推荐

MATLAB实现控制系统的时域分析

目的用 MATLAB 对控制系统进行时域分析，包括典型响应、判断系统稳 定性和分析系统的动态特性。理论二、典型响应及其性能分析1、单位阶跃响应单位阶跃响应调用格式为：(1) step(num,den)(2) step(num,den,t)(3) step(G)(4) step(G,t)该函数将绘制出系统在单位阶跃输入条件下的动态响应图，同时给出稳态值。其中 t 为图像

avatar 开放原子开发者工作坊

【深度学习2】基于Pytorch的WGAN理论和代码解析

参考文章：令人拍案叫绝的Wasserstein GAN - 知乎 (zhihu.com)1 原始GAN存在问题实际训练中，GAN存在着训练困难、生成器和判别器的loss无法指示训练进程、生成样本缺乏多样性等问题。这与GAN的机制有关。GAN最终达到对抗的纳什均衡只是一个理想状态，而现实情况中得到的结果都是中间状态（伪平衡）。大部分的情况是，随着训练的次数越多判别器D的效果越好，会导致一直可以将生成

avatar 开放原子开发者工作坊

RocketMQ的顺序消费问题MessageListenerOrderly()

consumer.registerMessageListener(new MessageListenerOrderly() {@Overridepublic ConsumeOrderlyStatus consumeMessage(List<MessageExt> msgs, ConsumeOrderlyContext context) {for (MessageExt msg : ms

avatar 开放原子开发者工作坊