STM32双轮平衡小车（基于STM32F103C8T6HAL库）

STM32双轮平衡小车参考教程

这个项目是跟做以上UP的STM32双轮平衡小车，主要是为了学习电机驱动和PID控制。这篇我就不提供源码了，我也是跟学的，原作者也提供了源码，我记录一下自己的理解。

1 PID原理

1.1 PID简介

1.2 PID演示

KP：比例环节：向目标靠近，但是在目标附近振荡

KI：积分环节：消除稳态误差

KD：微分环节：减小振荡

（1）无任何控制

上图中，可以看到中间是一个小球和绿色的板子，我们点击修改目标后波动板子位置。如下图所示：

然后我们切换到推动小球模式下，鼠标波动一下小球，可以看到下图所示结果：

在没有任何控制的情况的下，小球会跑飞。

（2）施加KP控制

首先要给一个输出限幅，这个输出限幅是一定要加的。在实际应用中，比如说要控制电机，电机转动是有一个速度上限的，我们先给个10，然后KP给个0.1。会产生在这样的情况，小球会以目标为中心，在目标周围振荡，并且振荡幅度会越变越大，并且KP值越大，振荡幅度越大。

所以KP的作用就是能够让小球接近目标，但是会发生震荡，并且振荡幅度会越来越大。

（3）施加KD控制

为了减小KP产生的振荡，我们要引入微分环节KD，就是求导，这里对位置求导实际上就是速度，相当于引入了一个阻尼力，减小振荡。

引入KD以后，我们会发现振荡幅度远远减小，并且KD越大，幅度越小。具体表现为KD小的情况下作用弱，要振荡多次才能停下来，KD大的时候阻尼力变大，振荡次数减少。但是也不是越大越好，因为在实际系统中，可能受到其它因素的干扰，KD过大会将这些干扰放大，造成小球在本该静止大的位置发生抽搐现象。存在的问题就是目标位置和当前位置有一定差距，就是使用KD控制没办法减小系统平衡时和目的之间的差距，即稳态误差。

（4）引入KI控制

为了消除稳态误差，需要引入KI环节。因为PD控制器是和时间无关的，因此在整个系统达到稳定的过程中，没办法消除累计误差，因此需要引入KI环节。

1.3 平衡小车PID控制原理

1.3.1 直立控制

我们先看一下结论：

我们为什么要得到这样的结论？如何得到这样的结论？

中立位:这个中立位是个相对的概念，我们可能会认为中立位是小车直立（θ=0）的时候，实际上只要小车能够保持平衡不请到都是中立位。比如说我的小车要前进，那必然不可能保持完全直立，必须要有一定的θ才能前进。

$a=k_{1}\theta +k_{2}\theta ^{'}$ :这是不是就是KP+KD（比例+微分）控制，KP让小车能够趋于平衡，KD减小振荡、缩短小车达到平衡的时间。

现在结论就来了：可以这么理解，上面标红色的那个公式是小车中立位的状态，而这个结论就是小车从非中立位靠近中立位的过程。

2 双轮平衡小车各模块驱动

草履虫都能学会的STM32平衡小车教程（基础篇）_哔哩哔哩_bilibili

此项目的重点是PID控制，因此关于各外设不详细展开，这位UP提供的源码里面也有，所以重点记录PID的使用。

2.1 硬件原理图

2.2 OLED屏幕

因为这个外设江科大的讲的更详细，就不单独记录了。

参考：

[模块教程] 第1期 0.96寸OLED显示屏_哔哩哔哩_bilibili

7.OLED显示_哔哩哔哩_bilibili

2.3 MPU6050

这个模块江科大讲的更详细，就不单独记录了。

参考：

江协科技STM32学习笔记（第09章 I2C通信)_xcl、xda-CSDN博客

8.MPU6050读取角度_哔哩哔哩_bilibili

2.4 超声波测距模块

超声波测距模块HC-SR04（基于STM32F103C8T6HAL库）-CSDN博客

2.5 电机驱动及编码器测速

电机驱动及编码器测速（基于STM32F103C8T6HAL库）-CSDN博客

2.6 蓝牙串口模块JDY-31

蓝牙串口模块JDY-31（基于STM32F103C8T6HAL库）-CSDN博客

3 PID控制

在实现以上驱动部分基础上实现PID控制。

3.1 PID程序编写

创建pid.c和pid.h

3.1.1 直立环PD控制器

/*直立环PD控制器
输入：期望角度、真实角度、角速度
期望角度：通过外环速度环控制器获得，
真实角度：通过MPU6050陀螺仪获得，
角速度：通过MPU6050陀螺仪获得
*/
int Vertical(float Med,float Angle,float gyro_Y)
{
	int temp;
	temp=Vertical_Kp*(Angle-Med)+Vertical_Kd*gyro_Y;
	return temp;                             //直立环的输出会反馈给速度环，所以要保持跟速度环输入一样，速度环输入是电机的PWM
}

3.1.2 速度环PI控制器

/*速度环PI控制器
输入：期望速度、左编码器测速、右编码器测速
*/
int Velocity(int Target,int encoder_L,int encoder_R)
{
	static int Err_LowOut_last,Encoder_S;
	static float a=0.7;
	int Err,Err_LowOut,temp;
	Velocity_Ki=Velocity_Kp/200;
	//1、计算偏差值
	Err=(encoder_L+encoder_R)-Target;           
	//这里应该是为了方便这样写的，因为在平衡位置，我们的小车左右速度应该是一样的，可以单独计算，为了方便，直接相加计算了
	//2、低通滤波
	Err_LowOut=(1-a)*Err+a*Err_LowOut_last;
	Err_LowOut_last=Err_LowOut;
	//3、积分
	Encoder_S+=Err_LowOut;
	//4、积分限幅(-20000~20000)，防止积分值过大影响P环节
	Encoder_S=Encoder_S>20000?20000:(Encoder_S<(-20000)?(-20000):Encoder_S); 
	/*因为无法直接获得积分值，所以以上几步就是用来计算积分值的*/
	if(stop==1)Encoder_S=0,stop=0;       
	/*蓝牙下发停止指令后，让积分值清0，状态清0，给下一次使用做准备，因为下发指令后；如果有积分影响，会导致按下停止按钮后，
	不会立刻停下来，会先往前，然后再退回*/
	//5、速度环计算
	temp=Velocity_Kp*Err_LowOut+Velocity_Ki*Encoder_S;
	return temp;
}

3.1.3 转向环PD控制器

/*转向环PD控制器
输入：角速度、目标角度值
角速度：MPU6050测得
目标角度值：在这个值是相对的，给90°就表示小车你要给我转90°
输出：电机PWM改变值，使用差速转向，转向时两个轮子转速一个高一个低
*/
int Turn(float gyro_Z,int Target_turn)
{
	int temp;
	temp=Turn_Kp*Target_turn+Turn_Kd*gyro_Z;
	return temp;
}

3.2 核心闭环控制函数

闭环控制函数就是将以上几个控制器结合到一起。

先motor.c在里添加电机速度限幅函数和电机停止函数。

#define PWM_MAX 7200
#define PWM_MIN -7200
extern uint8_t stop;

/*电机限幅函数*/
void Limit(int *motoA,int *motoB)
{
	if(*motoA>PWM_MAX)*motoA=PWM_MAX;
	if(*motoA<PWM_MIN)*motoA=PWM_MIN;
	if(*motoB>PWM_MAX)*motoB=PWM_MAX;
	if(*motoB<PWM_MIN)*motoB=PWM_MIN;
}

/*电机停止函数*/
void Stop(float *Med_Jiaodu,float *Jiaodu)
{
	if(abs((int)(*Jiaodu-*Med_Jiaodu))>60)
	{
		Load(0,0);
		stop=1;
	}
}

pid.c：

/*闭环控制函数：将上面几个控制器整合到一块*/
void Control(void)	//每隔10ms调用一次
{
	int PWM_out;
	//1、读取编码器和陀螺仪的数据
	Encoder_Left=Read_Speed(&htim2);           //读取编码器测速  
	Encoder_Right=-Read_Speed(&htim4);        
	mpu_dmp_get_data(&pitch,&roll,&yaw);       //读取姿态角
	MPU_Get_Gyroscope(&gyrox,&gyroy,&gyroz);   //读取角速度值
	MPU_Get_Accelerometer(&aacx,&aacy,&aacz);  //读取加速度值
	
	//2、将数据传入PID控制器，计算输出结果，即左右电机转速值
	Velocity_out=Velocity(Target_Speed,Encoder_Left,Encoder_Right);   //速度环输出
	Vertical_out=Vertical(Velocity_out+Med_Angle,roll,gyrox);         //后面两个参数与MPU6050安装方向有关
	Turn_out=Turn(gyroz,Target_turn);                                 //转向换输出
	PWM_out=Vertical_out;                //直立环输出后的参数就是电机的PWM控制
	//差速转向控制
	MOTO1=PWM_out-Turn_out;
	MOTO2=PWM_out+Turn_out;              
	Limit(&MOTO1,&MOTO2);                //PWM的最大转速是`7200和7200
	Load(MOTO1,MOTO2);
	Stop(&Med_Angle,&roll);//安全检测
}

3.3 利用MPU6050产生中断

现在需要完成的工作就是每隔一段时间进行一次采样和PID控制了，因此需要比较精准的定时器，这里采用MPU6050的INT引脚来进行，每当MPU6050采样好，就会输出一个低电平，告诉单片机采样好了。

每隔10ms采样一次的话，就设置MPU6050的采样率:

MPU_Set_Rate(100);						//设置采样率100Hz

这里需要配置PB5引脚为外部中断模式，并配置为上拉模式，下降沿触发方式，每隔10ms拉低一次电平上报采样数据给单片机。

先初始化陀螺仪再开启中断是因为我们读取数据的前提是陀螺仪初始化好，不然读不出来，所以要先初始化陀螺仪再开中断。

更改中断回调函数：

我们之前在超声波测距模块里调用了中断回调函数，使用HAL库配置的项目中断回调函数是共用的，所以要在这个函数里区别一下是哪个引脚产生的中断。

void HAL_GPIO_EXTI_Callback(uint16_t GPIO_Pin)
{
	if(GPIO_Pin==GPIO_PIN_2)
	{
		if(HAL_GPIO_ReadPin(GPIOA,GPIO_PIN_2)==GPIO_PIN_SET)
		{
			__HAL_TIM_SetCounter(&htim3,0);
			HAL_TIM_Base_Start(&htim3);
		}
		else
		{
			HAL_TIM_Base_Stop(&htim3);
			count=__HAL_TIM_GetCounter(&htim3);
			distance=count*0.017;
		}
	}
	if(GPIO_Pin==GPIO_PIN_5)
		Control();
}

4 PID调参

需要调以下几个参数

4.1 机械中值的确定

小车组装好后，将代码下载到单片机里，让小车分别向前向后倾倒，观察倾倒时候的角度值。

$Med_Angle =(\theta _{1}+\theta _{2})/2$

这个值可能每次开机都不一样，大概就行。

4.2 直立环数量级的确定

（1）Kp数量级

我们的输出是电机的PWM，电机的PWM在-7200~7200之间，角度大概是在0~60°，我们取个中间值30。

7200/30	240
7200/10	720

所以Kp大概在0~1000之间。

（2）Kd数量级

Kd就是角度的微分，即角速度，所以我们可以直接用OLED显示进行观察一下，这里为什么是gyrox呢，因为我们的小车前倾和后倾是绕x轴转动的。

经过观察，这个gyrox大概在2000~3000的话，7200/2000=3.6,所以Kd的数量级大概在0~10之间。

4.3 速度环参数数量级的确定

速度环的输出是一个角度值，输入是7200的PWM，但是我们的输入是编码器测得的速度，所以我们可以直接加载一个满量程转速测一下。

可以测得PWM满量程的时候，编码器测得的转速大概为70左右。所以编码器测得的值会在0~70之间。

$\frac{0~70}{70+70}=0.5$

所以Kp大概在0~1之间，Ki根据工程经验得出，约为Kp/200。

4.4 直立环参数极性的确定

我们可以单独给要判断赋一个初值，小车向前倾车轮向前转就是正确极性，极性反了就会导致小车向前倾，轮子向后转。

4.5 速度环参数极性的确定

先给直立环赋一个比较小的值，然后速度环给一个较大值：

极性正确的情况下往前倾会车轮转速会变快，极性反了车轮会转得比较慢。

4.6 直立环调参

（1）Kp调参

我们在其它参数没有得时候先给一个Kp。

观察现象：下载后朝某一个方向压一下小车，如果现象是小车移动时候倾角很明显，那就是Kp太小了，如果现象是小车振荡幅度很大，那就是超调了，Kp给的太大。反复测试得到一个合适的值，先调到一个明显临界超调的状态。

（2）Kd调参

Kd得目的是为了消除小车平衡时候的振荡，先调到一个明显临界超调的状态。

以上两个值是我们是使用明显临界超调的时候调出来的参数，所以最终我们还需要乘以一个系数0.6。

4.7 速度环调参

在上面参数的基础上，先给速度环Kp一个参数0.4，Ki为Kp/200，然后反复修改，直至没有显著振荡。

4.8 转向环调参

这个转向换并不是一个严格的PD控制器，不是那种目标角度值减去实际角度值的格式，因为陀螺仪测量转向角（偏航角）这个角度的时候，测的不是很准，误差非常大，因为陀螺仪在做DMP的时候，是累计误差的，是把时间上的误差都累积起来，所以算出来的偏航角误差非常大，所以没办法直接用这个角度。这里Targe_turn直接是目标的转向速度，Kp环节主要负责遥控转向，Kp值起到放大作用，Kp越大转向越快，这个值不是特别重要；Kd主要是为了保证转向时候走直线，Kd的数量值与速度环Ki量级是一样的，一般取一个经验值0.6。

5 蓝牙遥控

这一节就是为了实现手机蓝牙串口助手下发指令控制小车前后左右、停止运行。

初始化的时候要先打开串口才能接收数据

void USART3_IRQHandler(void)
{
  /* USER CODE BEGIN USART3_IRQn 0 */

  /* USER CODE END USART3_IRQn 0 */
  HAL_UART_IRQHandler(&huart3);
  /* USER CODE BEGIN USART3_IRQn 1 */
	Bluetooth_data=rx_buf[0];
	if(Bluetooth_data==0x00)		 Fore=0,Back=0,Left=0,Right=0;//刹
	else if(Bluetooth_data==0x01)Fore=1,Back=0,Left=0,Right=0;//前
	else if(Bluetooth_data==0x05)Fore=0,Back=1,Left=0,Right=0;//后
	else if(Bluetooth_data==0x03)Fore=0,Back=0,Left=0,Right=1;//右
	else if(Bluetooth_data==0x07)Fore=0,Back=0,Left=1,Right=0;//左
	else												 Fore=0,Back=0,Left=0,Right=0;//刹
	HAL_UART_Receive_IT(&huart3,rx_buf,1);
  /* USER CODE END USART3_IRQn 1 */
}

这里的值要跟手机蓝牙串口设置的一样，下发指令后才会正常响应。

/*闭环控制函数：将上面几个控制器整合到一块*/
void Control(void)	//每隔10ms调用一次
{
	int PWM_out;
	//1、读取编码器和陀螺仪的数据
	Encoder_Left=Read_Speed(&htim2);           //读取编码器测速  
	Encoder_Right=-Read_Speed(&htim4);        
	mpu_dmp_get_data(&pitch,&roll,&yaw);       //读取姿态角
	MPU_Get_Gyroscope(&gyrox,&gyroy,&gyroz);   //读取角速度值
	MPU_Get_Accelerometer(&aacx,&aacy,&aacz);  //读取加速度值
	//遥控
	if((Fore==0)&&(Back==0))Target_Speed=0;//未接受到前进后退指令-->速度清零，稳在原地
	if(Fore==1)
	{
		if(distance<50)
			Target_Speed++;                 //这个先+还是先-要根据极性做出相应改变
		else
			Target_Speed--;
	}
	if(Back==1){Target_Speed++;}//
	Target_Speed=Target_Speed>SPEED_Y?SPEED_Y:(Target_Speed<-SPEED_Y?(-SPEED_Y):Target_Speed);//限幅
	
	/*左右*/
	if((Left==0)&&(Right==0))Target_turn=0;
	if(Left==1)Target_turn+=30;	//左转
	if(Right==1)Target_turn-=30;	//右转
	Target_turn=Target_turn>SPEED_Z?SPEED_Z:(Target_turn<-SPEED_Z?(-SPEED_Z):Target_turn);//限幅( (20*100) * 100   )
	
	/*转向约束*/
	if((Left==0)&&(Right==0))Turn_Kd=0.6;//若无左右转向指令，则开启转向约束
	else if((Left==1)||(Right==1))Turn_Kd=0;//若左右转向指令接收到，则去掉转向约束

	
	//2、将数据传入PID控制器，计算输出结果，即左右电机转速值
	Velocity_out=Velocity(Target_Speed,Encoder_Left,Encoder_Right);   //速度环输出
	Vertical_out=Vertical(Velocity_out+Med_Angle,roll,gyroy);         //后面两个参数与MPU6050安装方向有关
	Turn_out=Turn(gyroz,Target_turn);                                 //转向换输出
	PWM_out=Vertical_out;                //直立环输出后的参数就是电机的PWM控制
	//差速转向控制
	MOTO1=PWM_out-Turn_out;
	MOTO2=PWM_out+Turn_out;              
	Limit(&MOTO1,&MOTO2);                //PWM的最大转速是`7200和7200
	Load(MOTO1,MOTO2);
	Stop(&Med_Angle,&roll);//安全检测
}

6 最终代码

（1）main.c

/* USER CODE BEGIN Header */
/**
  ******************************************************************************
  * @file           : main.c
  * @brief          : Main program body
  ******************************************************************************
  * @attention
  *
  * Copyright (c) 2024 STMicroelectronics.
  * All rights reserved.
  *
  * This software is licensed under terms that can be found in the LICENSE file
  * in the root directory of this software component.
  * If no LICENSE file comes with this software, it is provided AS-IS.
  *
  ******************************************************************************
  */
/* USER CODE END Header */
/* Includes ------------------------------------------------------------------*/
#include "main.h"
#include "i2c.h"
#include "tim.h"
#include "usart.h"
#include "gpio.h"

/* Private includes ----------------------------------------------------------*/
/* USER CODE BEGIN Includes */
#include "oled.h"
#include "IIC.h"
#include "inv_mpu.h"
#include "inv_mpu_dmp_motion_driver.h"
#include "mpu6050.h"
#include "stdio.h"
#include "sr04.h"
#include "motor.h"
#include "encoder.h"
/* USER CODE END Includes */

/* Private typedef -----------------------------------------------------------*/
/* USER CODE BEGIN PTD */

/* USER CODE END PTD */

/* Private define ------------------------------------------------------------*/
/* USER CODE BEGIN PD */
/* USER CODE END PD */

/* Private macro -------------------------------------------------------------*/
/* USER CODE BEGIN PM */

/* USER CODE END PM */

/* Private variables ---------------------------------------------------------*/

/* USER CODE BEGIN PV */
extern float roll;
extern int Encoder_Left,Encoder_Right;
uint8_t display_buf[20];
uint32_t sys_tick;
extern float distance;
extern uint8_t rx_buf[2];
/* USER CODE END PV */

/* Private function prototypes -----------------------------------------------*/
void SystemClock_Config(void);
/* USER CODE BEGIN PFP */
void Read(void);
/* USER CODE END PFP */

/* Private user code ---------------------------------------------------------*/
/* USER CODE BEGIN 0 */

/* USER CODE END 0 */

/**
  * @brief  The application entry point.
  * @retval int
  */
int main(void)
{
  /* USER CODE BEGIN 1 */

  /* USER CODE END 1 */

  /* MCU Configuration--------------------------------------------------------*/

  /* Reset of all peripherals, Initializes the Flash interface and the Systick. */
  HAL_Init();

  /* USER CODE BEGIN Init */

  /* USER CODE END Init */

  /* Configure the system clock */
  SystemClock_Config();

  /* USER CODE BEGIN SysInit */

  /* USER CODE END SysInit */

  /* Initialize all configured peripherals */
  MX_GPIO_Init();
  MX_I2C1_Init();
  MX_TIM3_Init();
  MX_TIM1_Init();
  MX_TIM2_Init();
  MX_TIM4_Init();
  MX_USART3_UART_Init();
  /* USER CODE BEGIN 2 */
	OLED_Init();
	OLED_Clear();
	MPU_Init();
	mpu_dmp_init();
	OLED_ShowString(0,00,"Init Sucess",16);
	HAL_NVIC_SetPriority(EXTI9_5_IRQn, 0, 0);
  HAL_NVIC_EnableIRQ(EXTI9_5_IRQn);
	HAL_TIM_PWM_Start(&htim1,TIM_CHANNEL_1);
	HAL_TIM_PWM_Start(&htim1,TIM_CHANNEL_4);
	HAL_TIM_Encoder_Start(&htim2,TIM_CHANNEL_ALL);
	HAL_TIM_Encoder_Start(&htim4,TIM_CHANNEL_ALL);
	HAL_UART_Receive_IT(&huart3,rx_buf,1);
	Load(0,0);
	OLED_Clear();
  /* USER CODE END 2 */

  /* Infinite loop */
  /* USER CODE BEGIN WHILE */
  while (1)
  {
		sprintf((char *)display_buf,"Encoder_L:%d   ",Encoder_Left);
		OLED_ShowString(0,0,display_buf,16);
		sprintf((char *)display_buf,"Encoder_R:%d   ",Encoder_Right);
		OLED_ShowString(0,2,display_buf,16);		
		sprintf((char *)display_buf,"roll:%.1f   ",roll); 
		OLED_ShowString(0,4,display_buf,16);
		GET_Distance();
		sprintf((char *)display_buf,"distance:%.1f  ",distance);
		OLED_ShowString(0,6,display_buf,12);
    /* USER CODE END WHILE */

    /* USER CODE BEGIN 3 */
  }
  /* USER CODE END 3 */
}

/**
  * @brief System Clock Configuration
  * @retval None
  */
void SystemClock_Config(void)
{
  RCC_OscInitTypeDef RCC_OscInitStruct = {0};
  RCC_ClkInitTypeDef RCC_ClkInitStruct = {0};

  /** Initializes the RCC Oscillators according to the specified parameters
  * in the RCC_OscInitTypeDef structure.
  */
  RCC_OscInitStruct.OscillatorType = RCC_OSCILLATORTYPE_HSE;
  RCC_OscInitStruct.HSEState = RCC_HSE_ON;
  RCC_OscInitStruct.HSEPredivValue = RCC_HSE_PREDIV_DIV1;
  RCC_OscInitStruct.HSIState = RCC_HSI_ON;
  RCC_OscInitStruct.PLL.PLLState = RCC_PLL_ON;
  RCC_OscInitStruct.PLL.PLLSource = RCC_PLLSOURCE_HSE;
  RCC_OscInitStruct.PLL.PLLMUL = RCC_PLL_MUL9;
  if (HAL_RCC_OscConfig(&RCC_OscInitStruct) != HAL_OK)
  {
    Error_Handler();
  }

  /** Initializes the CPU, AHB and APB buses clocks
  */
  RCC_ClkInitStruct.ClockType = RCC_CLOCKTYPE_HCLK|RCC_CLOCKTYPE_SYSCLK
                              |RCC_CLOCKTYPE_PCLK1|RCC_CLOCKTYPE_PCLK2;
  RCC_ClkInitStruct.SYSCLKSource = RCC_SYSCLKSOURCE_PLLCLK;
  RCC_ClkInitStruct.AHBCLKDivider = RCC_SYSCLK_DIV1;
  RCC_ClkInitStruct.APB1CLKDivider = RCC_HCLK_DIV2;
  RCC_ClkInitStruct.APB2CLKDivider = RCC_HCLK_DIV1;

  if (HAL_RCC_ClockConfig(&RCC_ClkInitStruct, FLASH_LATENCY_2) != HAL_OK)
  {
    Error_Handler();
  }
}

/* USER CODE BEGIN 4 */


/* USER CODE END 4 */

/**
  * @brief  This function is executed in case of error occurrence.
  * @retval None
  */
void Error_Handler(void)
{
  /* USER CODE BEGIN Error_Handler_Debug */
  /* User can add his own implementation to report the HAL error return state */
  __disable_irq();
  while (1)
  {
  }
  /* USER CODE END Error_Handler_Debug */
}

#ifdef  USE_FULL_ASSERT
/**
  * @brief  Reports the name of the source file and the source line number
  *         where the assert_param error has occurred.
  * @param  file: pointer to the source file name
  * @param  line: assert_param error line source number
  * @retval None
  */
void assert_failed(uint8_t *file, uint32_t line)
{
  /* USER CODE BEGIN 6 */
  /* User can add his own implementation to report the file name and line number,
     ex: printf("Wrong parameters value: file %s on line %d\r\n", file, line) */
  /* USER CODE END 6 */
}
#endif /* USE_FULL_ASSERT */

（2）motor.c

#include "motor.h"

#define PWM_MAX 7200
#define PWM_MIN -7200
extern TIM_HandleTypeDef htim1;
extern uint8_t stop;
int abs(int p)
{
	if(p>0)
		return p;
	else
		return -p;
}

void Load(int moto1,int moto2)			//-7200~7200
{
	if(moto1<0)
	{
		HAL_GPIO_WritePin(GPIOB,GPIO_PIN_13,GPIO_PIN_SET);
		HAL_GPIO_WritePin(GPIOB,GPIO_PIN_12,GPIO_PIN_RESET);
	}
	else
	{
		HAL_GPIO_WritePin(GPIOB,GPIO_PIN_13,GPIO_PIN_RESET);
		HAL_GPIO_WritePin(GPIOB,GPIO_PIN_12,GPIO_PIN_SET);
	}
	__HAL_TIM_SetCompare(&htim1,TIM_CHANNEL_4,abs(moto1));
	if(moto2<0)
	{
		HAL_GPIO_WritePin(GPIOB,GPIO_PIN_14,GPIO_PIN_SET);
		HAL_GPIO_WritePin(GPIOB,GPIO_PIN_15,GPIO_PIN_RESET);
	}
	else
	{
		HAL_GPIO_WritePin(GPIOB,GPIO_PIN_14,GPIO_PIN_RESET);
		HAL_GPIO_WritePin(GPIOB,GPIO_PIN_15,GPIO_PIN_SET);
	}
	__HAL_TIM_SetCompare(&htim1,TIM_CHANNEL_1,abs(moto2));
}

void Limit(int *motoA,int *motoB)
{
	if(*motoA>PWM_MAX)*motoA=PWM_MAX;
	if(*motoA<PWM_MIN)*motoA=PWM_MIN;
	if(*motoB>PWM_MAX)*motoB=PWM_MAX;
	if(*motoB<PWM_MIN)*motoB=PWM_MIN;
}
void Stop(float *Med_Jiaodu,float *Jiaodu)
{
	if(abs((int)(*Jiaodu-*Med_Jiaodu))>60)
	{
		Load(0,0);
		stop=1;
	}
}

（3）motor.h

#ifndef _MOTOR_H
#define _MOTOR_H

#include "stm32f1xx_hal.h"
void Load(int moto1,int moto2);
void Limit(int *motoA,int *motoB);
void Stop(float *Med_Jiaodu,float *Jiaodu);
#endif

（4）pid.c

#include "pid.h"
#include "encoder.h"
#include "inv_mpu.h"
#include "inv_mpu_dmp_motion_driver.h"
#include "mpu6050.h"
#include "motor.h"

//传感器数据变量
int Encoder_Left,Encoder_Right;
float pitch,roll,yaw;
short gyrox,gyroy,gyroz;
short	aacx,aacy,aacz;

//闭环控制中间变量
int Vertical_out,Velocity_out,Turn_out,Target_Speed,Target_turn,MOTO1,MOTO2;
float Med_Angle=3;//平衡时角度值偏移量（机械中值）
//参数
float Vertical_Kp=-120,Vertical_Kd=-0.8;			//直立环 数量级（Kp：0~1000、Kd：0~10）
float Velocity_Kp=-1.2,Velocity_Ki=-0.006;		//速度环 数量级（Kp：0~1）
float Turn_Kp=10,Turn_Kd=0.1;											//转向环

uint8_t stop;

extern TIM_HandleTypeDef htim2,htim4;
extern float distance;
extern uint8_t Fore,Back,Left,Right;
#define SPEED_Y 12 //俯仰(前后)最大设定速度
#define SPEED_Z 150//偏航(左右)最大设定速度 

//直立环PD控制器
//输入：期望角度、真实角度、角速度
int Vertical(float Med,float Angle,float gyro_Y)
{
	int temp;
	temp=Vertical_Kp*(Angle-Med)+Vertical_Kd*gyro_Y;
	return temp;
}

//速度环PI控制器
//输入：期望速度、左编码器、右编码器
int Velocity(int Target,int encoder_L,int encoder_R)
{
	static int Err_LowOut_last,Encoder_S;
	static float a=0.7;
	int Err,Err_LowOut,temp;
	Velocity_Ki=Velocity_Kp/200;
	//1、计算偏差值
	Err=(encoder_L+encoder_R)-Target;
	//2、低通滤波
	Err_LowOut=(1-a)*Err+a*Err_LowOut_last;
	Err_LowOut_last=Err_LowOut;
	//3、积分
	Encoder_S+=Err_LowOut;
	//4、积分限幅(-20000~20000)
	Encoder_S=Encoder_S>20000?20000:(Encoder_S<(-20000)?(-20000):Encoder_S);
	if(stop==1)Encoder_S=0,stop=0;
	//5、速度环计算
	temp=Velocity_Kp*Err_LowOut+Velocity_Ki*Encoder_S;
	return temp;
}


//转向环PD控制器
//输入：角速度、角度值
int Turn(float gyro_Z,int Target_turn)
{
	int temp;
	temp=Turn_Kp*Target_turn+Turn_Kd*gyro_Z;
	return temp;
}

void Control(void)	//每隔10ms调用一次
{
	int PWM_out;
	//1、读取编码器和陀螺仪的数据
	Encoder_Left=Read_Speed(&htim2);
	Encoder_Right=-Read_Speed(&htim4);
	mpu_dmp_get_data(&pitch,&roll,&yaw);
	MPU_Get_Gyroscope(&gyrox,&gyroy,&gyroz);
	MPU_Get_Accelerometer(&aacx,&aacy,&aacz);
	//遥控
	if((Fore==0)&&(Back==0))Target_Speed=0;//未接受到前进后退指令-->速度清零，稳在原地
	if(Fore==1)
	{
		if(distance<50)
			Target_Speed++;
		else
			Target_Speed--;
	}
	if(Back==1){Target_Speed++;}//
	Target_Speed=Target_Speed>SPEED_Y?SPEED_Y:(Target_Speed<-SPEED_Y?(-SPEED_Y):Target_Speed);//限幅
	
	/*左右*/
	if((Left==0)&&(Right==0))Target_turn=0;
	if(Left==1)Target_turn+=30;	//左转
	if(Right==1)Target_turn-=30;	//右转
	Target_turn=Target_turn>SPEED_Z?SPEED_Z:(Target_turn<-SPEED_Z?(-SPEED_Z):Target_turn);//限幅( (20*100) * 100   )
	
	/*转向约束*/
	if((Left==0)&&(Right==0))Turn_Kd=0.6;//若无左右转向指令，则开启转向约束
	else if((Left==1)||(Right==1))Turn_Kd=0;//若左右转向指令接收到，则去掉转向约束

	
	//2、将数据传入PID控制器，计算输出结果，即左右电机转速值
	Velocity_out=Velocity(Target_Speed,Encoder_Left,Encoder_Right);
	Vertical_out=Vertical(Velocity_out+Med_Angle,roll,gyrox);
	Turn_out=Turn(gyroz,Target_turn);
	PWM_out=Vertical_out;
	MOTO1=PWM_out-Turn_out;
	MOTO2=PWM_out+Turn_out;
	Limit(&MOTO1,&MOTO2);
	Load(MOTO1,MOTO2);
	Stop(&Med_Angle,&roll);//安全检测
}

（5）pid.h

#ifndef __PID_H__
#define __PID_H__

#include "stm32f1xx_hal.h"

void Control(void);	//每隔10ms调用一次

#endif

（6）stm32f1xx_it.c

/* USER CODE BEGIN Header */
/**
  ******************************************************************************
  * @file    stm32f1xx_it.c
  * @brief   Interrupt Service Routines.
  ******************************************************************************
  * @attention
  *
  * Copyright (c) 2024 STMicroelectronics.
  * All rights reserved.
  *
  * This software is licensed under terms that can be found in the LICENSE file
  * in the root directory of this software component.
  * If no LICENSE file comes with this software, it is provided AS-IS.
  *
  ******************************************************************************
  */
/* USER CODE END Header */

/* Includes ------------------------------------------------------------------*/
#include "main.h"
#include "stm32f1xx_it.h"
/* Private includes ----------------------------------------------------------*/
/* USER CODE BEGIN Includes */
/* USER CODE END Includes */

/* Private typedef -----------------------------------------------------------*/
/* USER CODE BEGIN TD */

/* USER CODE END TD */

/* Private define ------------------------------------------------------------*/
/* USER CODE BEGIN PD */

/* USER CODE END PD */

/* Private macro -------------------------------------------------------------*/
/* USER CODE BEGIN PM */

/* USER CODE END PM */

/* Private variables ---------------------------------------------------------*/
/* USER CODE BEGIN PV */
uint8_t rx_buf[2],Bluetooth_data;
uint8_t Fore,Back,Left,Right;
/* USER CODE END PV */

/* Private function prototypes -----------------------------------------------*/
/* USER CODE BEGIN PFP */

/* USER CODE END PFP */

/* Private user code ---------------------------------------------------------*/
/* USER CODE BEGIN 0 */

/* USER CODE END 0 */

/* External variables --------------------------------------------------------*/
extern UART_HandleTypeDef huart3;
/* USER CODE BEGIN EV */

/* USER CODE END EV */

/******************************************************************************/
/*           Cortex-M3 Processor Interruption and Exception Handlers          */
/******************************************************************************/
/**
  * @brief This function handles Non maskable interrupt.
  */
void NMI_Handler(void)
{
  /* USER CODE BEGIN NonMaskableInt_IRQn 0 */

  /* USER CODE END NonMaskableInt_IRQn 0 */
  /* USER CODE BEGIN NonMaskableInt_IRQn 1 */
  while (1)
  {
  }
  /* USER CODE END NonMaskableInt_IRQn 1 */
}

/**
  * @brief This function handles Hard fault interrupt.
  */
void HardFault_Handler(void)
{
  /* USER CODE BEGIN HardFault_IRQn 0 */

  /* USER CODE END HardFault_IRQn 0 */
  while (1)
  {
    /* USER CODE BEGIN W1_HardFault_IRQn 0 */
    /* USER CODE END W1_HardFault_IRQn 0 */
  }
}

/**
  * @brief This function handles Memory management fault.
  */
void MemManage_Handler(void)
{
  /* USER CODE BEGIN MemoryManagement_IRQn 0 */

  /* USER CODE END MemoryManagement_IRQn 0 */
  while (1)
  {
    /* USER CODE BEGIN W1_MemoryManagement_IRQn 0 */
    /* USER CODE END W1_MemoryManagement_IRQn 0 */
  }
}

/**
  * @brief This function handles Prefetch fault, memory access fault.
  */
void BusFault_Handler(void)
{
  /* USER CODE BEGIN BusFault_IRQn 0 */

  /* USER CODE END BusFault_IRQn 0 */
  while (1)
  {
    /* USER CODE BEGIN W1_BusFault_IRQn 0 */
    /* USER CODE END W1_BusFault_IRQn 0 */
  }
}

/**
  * @brief This function handles Undefined instruction or illegal state.
  */
void UsageFault_Handler(void)
{
  /* USER CODE BEGIN UsageFault_IRQn 0 */

  /* USER CODE END UsageFault_IRQn 0 */
  while (1)
  {
    /* USER CODE BEGIN W1_UsageFault_IRQn 0 */
    /* USER CODE END W1_UsageFault_IRQn 0 */
  }
}

/**
  * @brief This function handles System service call via SWI instruction.
  */
void SVC_Handler(void)
{
  /* USER CODE BEGIN SVCall_IRQn 0 */

  /* USER CODE END SVCall_IRQn 0 */
  /* USER CODE BEGIN SVCall_IRQn 1 */

  /* USER CODE END SVCall_IRQn 1 */
}

/**
  * @brief This function handles Debug monitor.
  */
void DebugMon_Handler(void)
{
  /* USER CODE BEGIN DebugMonitor_IRQn 0 */

  /* USER CODE END DebugMonitor_IRQn 0 */
  /* USER CODE BEGIN DebugMonitor_IRQn 1 */

  /* USER CODE END DebugMonitor_IRQn 1 */
}

/**
  * @brief This function handles Pendable request for system service.
  */
void PendSV_Handler(void)
{
  /* USER CODE BEGIN PendSV_IRQn 0 */

  /* USER CODE END PendSV_IRQn 0 */
  /* USER CODE BEGIN PendSV_IRQn 1 */

  /* USER CODE END PendSV_IRQn 1 */
}

/**
  * @brief This function handles System tick timer.
  */
void SysTick_Handler(void)
{
  /* USER CODE BEGIN SysTick_IRQn 0 */

  /* USER CODE END SysTick_IRQn 0 */
  HAL_IncTick();
  /* USER CODE BEGIN SysTick_IRQn 1 */

  /* USER CODE END SysTick_IRQn 1 */
}

/******************************************************************************/
/* STM32F1xx Peripheral Interrupt Handlers                                    */
/* Add here the Interrupt Handlers for the used peripherals.                  */
/* For the available peripheral interrupt handler names,                      */
/* please refer to the startup file (startup_stm32f1xx.s).                    */
/******************************************************************************/

/**
  * @brief This function handles EXTI line2 interrupt.
  */
void EXTI2_IRQHandler(void)
{
  /* USER CODE BEGIN EXTI2_IRQn 0 */

  /* USER CODE END EXTI2_IRQn 0 */
  HAL_GPIO_EXTI_IRQHandler(GPIO_PIN_2);
  /* USER CODE BEGIN EXTI2_IRQn 1 */

  /* USER CODE END EXTI2_IRQn 1 */
}

/**
  * @brief This function handles EXTI line[9:5] interrupts.
  */
void EXTI9_5_IRQHandler(void)
{
  /* USER CODE BEGIN EXTI9_5_IRQn 0 */

  /* USER CODE END EXTI9_5_IRQn 0 */
  HAL_GPIO_EXTI_IRQHandler(GPIO_PIN_5);
  /* USER CODE BEGIN EXTI9_5_IRQn 1 */

  /* USER CODE END EXTI9_5_IRQn 1 */
}

/**
  * @brief This function handles USART3 global interrupt.
  */
void USART3_IRQHandler(void)
{
  /* USER CODE BEGIN USART3_IRQn 0 */

  /* USER CODE END USART3_IRQn 0 */
  HAL_UART_IRQHandler(&huart3);
  /* USER CODE BEGIN USART3_IRQn 1 */
	Bluetooth_data=rx_buf[0];
	if(Bluetooth_data==0x00)		 Fore=0,Back=0,Left=0,Right=0;//刹
	else if(Bluetooth_data==0x01)Fore=1,Back=0,Left=0,Right=0;//前
	else if(Bluetooth_data==0x05)Fore=0,Back=1,Left=0,Right=0;//后
	else if(Bluetooth_data==0x03)Fore=0,Back=0,Left=0,Right=1;//右
	else if(Bluetooth_data==0x07)Fore=0,Back=0,Left=1,Right=0;//左
	else												 Fore=0,Back=0,Left=0,Right=0;//刹
	HAL_UART_Receive_IT(&huart3,rx_buf,1);
  /* USER CODE END USART3_IRQn 1 */
}

/* USER CODE BEGIN 1 */

/* USER CODE END 1 */

其余部分不需要进行更改，保持原来各模块的驱动原来状态即可。