无障碍轮椅——平衡功能代码解析

一、STM32引脚使用

引脚	功能
PA8	高级定时器输出PWM波：TIM1 CH1
PA11	高级定时器输出PWM波：TIM1 CH4
PB12	电机1方向位控制：TB6612 AIN1
PB13	电机1方向位控制：TB6612 AIN2
PB14	电机2方向位控制：TB6612 BIN1
PB15	电机2方向位控制：TB6612 BIN2
PA0	TIM2功能引脚：编码器1A相/B相
PA1	TIM2功能引脚：码器1A相/B相
PB6	TIM4功能引脚：编码器2A相/B相
PB7	TIM4功能引脚：编码器2A相/B相
PB10	串口功能引脚：TX
PB11	串口功能引脚：RX
PB5	接MPU6050 INT中断引脚输出端
PB4	MPU6050的I2C通信的SCL
PB3	MPU6050的I2C通信的SDA

二、PID算法

1. 位置式PID

• 适用于开关式闭环，例如温控系统，平衡小车直立环。

• 理论分析：位置闭环控制就是根据编码器的脉冲累加测量电机的位置信息，并与目标值进行比较，得到控制偏差，然后通过对偏差的比例、积分、微分进行控制，使偏差趋向于零的过程。

• 控制系统原理图
  

• 一般在定时中断函数里面调用该函数，因为执行时间需要是确定的，保证积分和差分的时序是固定的。

• 调参注意事项：在调参时要先确定Kp的值再确定Ki及Kd。

/**************************************************************************
函数功能：位置式PID控制器
入口参数：测量值，目标值
返回值：输出temp
根据位置式离散PID公式 ：
pwm=Kp*e(k)+Ki*∑e(k)+Kd[e（k）-e(k-1)]
参数含义：
- e(k)代表本次偏差 
- e(k-1)代表上一次的偏差  
- ∑e(k)代表e(k)以及之前的偏差的累积和;其中k为1,2,,k;
- pwm代表输出
各项含义：
- 比例项输出=本次偏差乘以比例项系数
- 积分项输出=历史偏差之和乘以积分项系数
- 微分项输出=最近两次偏差之差（上次偏差-本次偏差）乘以微分项系数
- PID输出：上述输出之和，也就是多少秒高电平
- PID计算时间（采样周期）要大于PWM周期时间
**************************************************************************/
int Position_PID (int Newpoint,int Target)
{   
     float Position_KP=80,Position_KI=0.1,Position_KD=500;
     static float Bias,temp,Integral_bias,Last_Bias;
     Bias=Newpoint-Target;                                  //求出偏差，由测量值减去目标值。
     Integral_bias+=Bias;                                    //求出偏差的积分
     temp=Position_KP*Bias+Position_KI*Integral_bias+Position_KD*(Bias-Last_Bias);       //位置式PID控制器
     Last_Bias=Bias;                                       //保存上一次偏差 
     return temp;                                           //增量输出
}

2. 增量式PID

• 适用于速度闭环，电流闭环等，受到偶然误差的影响较小，适用于连续系统。
• 控制系统原理图
  
• 一般在定时中断函数里面调用该函数，因为执行时间需要是确定的，保证积分和差分的时序是固定的。
• 调参注意事项：在调参时要先确定Ki的值在确定Kp及Kd。ps：因为直接乘以误差的那一项系数对系统影响最大

/**************************************************************************
函数功能：增量PI控制器(temp+=Kp[e（k）-e(k-1)]+Ki*e(k))
入口参数：测量值，目标值
返回  值：输出temp
根据增量式离散PID公式 ：
temp+=Kp[e（k）-e(k-1)]+Ki*e(k)+Kd[e(k)-2e(k-1)+e(k-2)]
e(k)代表本次偏差 
e(k-1)代表上一次的偏差 
e(k-2)代表上上一次的偏差 
temp代表增量输出
temp+=Kp[e（k）-e(k-1)]+Ki*e(k)
**************************************************************************/
int Incremental_PID (int Nowpoint,int Target)
{   
     float Kp=20,Ki=30,Kd=10;   
     static int Bias,temp,Last_bias,Prev_bias;         //相关内部变量的定义。
     Bias=Nowpoint-Target;                //求出偏差，由测量值减去目标值。
     temp+=Kp*(Bias-Last_bias)+Ki*Bias+Kd*(Bias-2*Last_bias+Prev_bias);   //使用增量 PI 控制器
     Prev_bias=Last_bias;					 //保存上上一次偏差 
	 Last_bias=Bias;                       //保存上一次偏差 
     return temp;                         //增量输出
}

三、主要文件与函数功能介绍

1. pwm.c

• 输出两路PWM波（驱动电机） 高级定时器TIM1 CH1/CH4----------PA8/PA11
• 连接TB6612的PWMA/PWMB

/*
输入参数：自动重装器Per，预分频器Psc
返回值：无
调用函数PWM_Init_TIM1(0,7199);//0分频，周期为7199 也就是PWM波的频率为10K，电机的最大频率就是10K

*/
void PWM_Init_TIM1(u16 Psc,u16 Per)
{

	RCC_APB2PeriphClockCmd(RCC_APB2Periph_GPIOA | RCC_APB2Periph_TIM1 | RCC_APB2Periph_AFIO,ENABLE);//开启时钟
	
	//初始化引脚PA8，PA11
	GPIO_InitTypeDef GPIO_InitStruct;
	GPIO_InitStruct.GPIO_Mode=GPIO_Mode_AF_PP;//复用推挽输出
	GPIO_InitStruct.GPIO_Pin=GPIO_Pin_8 |GPIO_Pin_11;
	GPIO_InitStruct.GPIO_Speed=GPIO_Speed_50MHz;
	GPIO_Init(GPIOA,&GPIO_InitStruct);
	
	//高级定时器TIM1时基单元初始化
	TIM_TimeBaseInitTypeDef TIM_TimeBaseInitStruct;
	TIM_TimeBaseStructInit(&TIM_TimeBaseInitStruct);//初始化定时器。
	TIM_TimeBaseInitStruct.TIM_ClockDivision=TIM_CKD_DIV1;
	TIM_TimeBaseInitStruct.TIM_CounterMode=TIM_CounterMode_Up;
	TIM_TimeBaseInitStruct.TIM_Period=Per;
	TIM_TimeBaseInitStruct.TIM_Prescaler=Psc;
	TIM_TimeBaseInit(TIM1,&TIM_TimeBaseInitStruct);
	
	//高级定时器TIM1输出比较初始化
	TIM_OCInitTypeDef TIM_OCInitStruct;
	TIM_OCInitStruct.TIM_OCMode=TIM_OCMode_PWM1;//输出比较模式为PWM模式1
	TIM_OCInitStruct.TIM_OCPolarity=TIM_OCPolarity_High;//输出比较极性，该极性表示不翻转
	TIM_OCInitStruct.TIM_OutputState=TIM_OutputState_Enable;//设置输出使能
	TIM_OCInitStruct.TIM_Pulse=0;//CCR寄存器初始值
	TIM_OC1Init(TIM1,&TIM_OCInitStruct);//OC1通道初始化
	TIM_OC4Init(TIM1,&TIM_OCInitStruct);//OC4通道初始化
	
	//高级定时器写法与通用定时器不同
	TIM_CtrlPWMOutputs(TIM1,ENABLE);//高级定时器专属--MOE主输出使能
	
	TIM_OC1PreloadConfig(TIM1,TIM_OCPreload_Enable);//OC1预装载寄存器使能
	TIM_OC4PreloadConfig(TIM1,TIM_OCPreload_Enable);//OC4预装载寄存器使能
	TIM_ARRPreloadConfig(TIM1,ENABLE);//TIM1在ARR上预装载寄存器使能
	
	TIM_Cmd(TIM1,ENABLE);//开定时器。
}

参考博文：
STM32——TIM输出比较

2. motor.c

• 电机旋转方向位初始化
  • 电机1——PB12/PB13 ——TB6612 AIN1/AIN2
  • 电机2——PB14/PB15 ——TB6612 BIN1/BIN2

void Motor_Init(void)
{
	
	RCC_APB2PeriphClockCmd(RCC_APB2Periph_GPIOB,ENABLE);//开启时钟
	
	GPIO_InitTypeDef GPIO_InitStruct;
	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);	
}

• 绝对值函数【因为两个电机安装方向相反】

/*绝对值函数*/
int abs(int p)
{
	int q;
	q=p>0?p:(-p);
	return q;
}

• PWM赋值函数

/*
入口参数：PID运算完成后的最终PWM值
*/
#define Ain1 PBout(14);
#define Ain2 PBout(15);
#define Bin1 PBout(13);
#define Bin2 PBout(12);

void Load(int moto1,int moto2)
{
	//1.研究正负号，对应正反转
	if(moto1>0)	
		Ain1=1,Ain2=0;//正转
	else 				
		Ain1=0,Ain2=1;//反转
	//2.研究PWM值
	TIM_SetCompare1(TIM1,abs(moto1));
	
	if(moto2>0)	
		Bin1=1,Bin2=0;
	else 				
		Bin1=0,Bin2=1;	
	TIM_SetCompare4(TIM1,abs(moto2));
}

• PWM限幅

/*
pwm最大值对应电机最大功率
定时器频率为72MHZ，计数7200次，周期为10k，对应电机最大功率，
*/
int PWM_MAX=7200,PWM_MIN=-7200;	//PWM限幅变量

/*限幅函数*/
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;
}

3. encoder.c

• 编码器模式初始化以及输入捕获模式配置
  • 编码器1——PA0/PA1——TIM2 A相/B相
  • 编码器2——PB6/PB7——TIM4 A相/B相

/*
编码器模式配置为TI12模式：在T1和T2的所有边沿计数以及都不返相
配置通用定时器2和4，编码器接口只能是CH1和CH2引脚

*/
void Encoder_TIM2_Init(void)
{

	RCC_APB2PeriphClockCmd(RCC_APB2Periph_GPIOA,ENABLE);//开启时钟
	RCC_APB1PeriphClockCmd(RCC_APB1Periph_TIM2,ENABLE);
	
	GPIO_InitTypeDef GPIO_InitStruct;
	GPIO_InitStruct.GPIO_Mode=GPIO_Mode_IN_FLOATING;//浮空输入
	GPIO_InitStruct.GPIO_Pin=GPIO_Pin_0 |GPIO_Pin_1;
	GPIO_Init(GPIOA,&GPIO_InitStruct);
	
	TIM_TimeBaseInitTypeDef TIM_TimeBaseInitStruct;
	TIM_TimeBaseStructInit(&TIM_TimeBaseInitStruct);//初始化定时器。
	TIM_TimeBaseInitStruct.TIM_ClockDivision=TIM_CKD_DIV1;
	TIM_TimeBaseInitStruct.TIM_CounterMode=TIM_CounterMode_Up;
	TIM_TimeBaseInitStruct.TIM_Period=65535;//ARR
	TIM_TimeBaseInitStruct.TIM_Prescaler=0;//PSC，不分频，编码器时钟直接驱动计数器
	TIM_TimeBaseInit(TIM2,&TIM_TimeBaseInitStruct);
	
	TIM_ICInitTypeDef TIM_ICInitStruct;
	TIM_ICStructInit(&TIM_ICInitStruct);//初始化输入捕获通道1和2
	TIM_ICInitStruct.TIM_ICFilter=10;//滤波器
	TIM_ICInit(TIM2,&TIM_ICInitStruct);
	
	TIM_EncoderInterfaceConfig(TIM2,TIM_EncoderMode_TI12,TIM_ICPolarity_Rising,TIM_ICPolarity_Rising);//配置编码器模式，第二个参数为编码器工作模式，参数三四为通道的极性，Falling表示通道反向，Rising表示通道不反向
	
	TIM_ClearFlag(TIM2,TIM_FLAG_Update);//清除溢出更新中断标志位
	TIM_ITConfig(TIM2,TIM_IT_Update,ENABLE);//配置溢出更新中断标志位
	
	TIM_SetCounter(TIM2,0);//清零定时器计数值
	
	TIM_Cmd(TIM2,ENABLE);//开启定时器
}


void Encoder_TIM4_Init(void)
{
	
	RCC_APB2PeriphClockCmd(RCC_APB2Periph_GPIOB,ENABLE);
	RCC_APB1PeriphClockCmd(RCC_APB1Periph_TIM4,ENABLE);
	
	GPIO_InitTypeDef GPIO_InitStruct;
	GPIO_InitStruct.GPIO_Mode=GPIO_Mode_IN_FLOATING;
	GPIO_InitStruct.GPIO_Pin=GPIO_Pin_6 |GPIO_Pin_7;
	GPIO_Init(GPIOB,&GPIO_InitStruct);
	
	TIM_TimeBaseInitTypeDef TIM_TimeBaseInitStruct;
	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(TIM4,&TIM_TimeBaseInitStruct);
	
	TIM_ICInitTypeDef TIM_ICInitStruct;
	TIM_ICStructInit(&TIM_ICInitStruct);
	TIM_ICInitStruct.TIM_ICFilter=10;
	TIM_ICInit(TIM4,&TIM_ICInitStruct);
	
	TIM_EncoderInterfaceConfig(TIM4,TIM_EncoderMode_TI12,TIM_ICPolarity_Rising,TIM_ICPolarity_Rising);
	
	TIM_ClearFlag(TIM4,TIM_FLAG_Update);
	TIM_ITConfig(TIM4,TIM_IT_Update,ENABLE);
	
	TIM_SetCounter(TIM4,0);

	TIM_Cmd(TIM4,ENABLE);
}

• 读取速度函数
  • 因为MPU6050引脚中断时10ms，我们将速度读取函数写在该中断中，时间很短，就不可能会产生溢出，所以不需要写该定时器中断处理函数，
  • TIM_GetCounter（）函数读取到的是脉冲数，相邻两个脉冲值相减才是单位时间的位移量，
    可以通过将寄存器清零TIM_SetCounter(TIM2,0)的方式得到单位时间的位移量，该方法更加简便。我们就以当前的计数值去近似作为他的速度。作为PID速度环的入口参数。

int Read_Speed(int TIMx)
{
	int value_1;
	switch(TIMx)
	{
		case 2:
			value_1=(short)TIM_GetCounter(TIM2);
			TIM_SetCounter(TIM2,0);break;//读取速度需要将定时器的计数值清零，读取位置不需要清零
		case 4:
			value_1=(short)TIM_GetCounter(TIM4);
			TIM_SetCounter(TIM4,0);break;
		default:
			value_1=0;
	}
	return value_1;
}

参考博文：
STM32——TIM编码器接口

4. oled.c

参考博文：
STM32——OLED调试工具与显示屏中OLED显示屏源码
STM32——I2C通信中第十三章：软件I2C读写MPU6050

5. usart3.c

• 蓝牙串口配置
  • TX——PB10
  • RX——PB11

/*
输入参数：波特率
*/
void uart3_init(u32 bound)
{
	RCC_APB2PeriphClockCmd(RCC_APB2Periph_GPIOB, ENABLE);//时钟GPIOB、USART3
	RCC_APB1PeriphClockCmd(RCC_APB1Periph_USART3, ENABLE);
	
	//GPIO端口设置
	//USART3_TX   PB10
	GPIO_InitTypeDef GPIO_InitStructure;
	GPIO_InitStructure.GPIO_Pin = GPIO_Pin_10;
	GPIO_InitStructure.GPIO_Speed = GPIO_Speed_50MHz;
	GPIO_InitStructure.GPIO_Mode = GPIO_Mode_AF_PP;//复用推挽输出
	GPIO_Init(GPIOB, &GPIO_InitStructure);
	//无须定义新的结构体配置成员，因为调用GPIO_Init（）函数后就写入到硬件寄存器中
	//USART3_RX	  PB11
	GPIO_InitStructure.GPIO_Pin = GPIO_Pin_11;
	GPIO_InitStructure.GPIO_Mode = GPIO_Mode_IN_FLOATING;//浮空输入
	GPIO_Init(GPIOB, &GPIO_InitStructure);  
	
	//USART 初始化设置
	USART_InitTypeDef USART_InitStructure;
	USART_InitStructure.USART_BaudRate = bound;//一般设置为9600;
	USART_InitStructure.USART_WordLength = USART_WordLength_8b;//设置数据位
	USART_InitStructure.USART_StopBits = USART_StopBits_1;//设置停止位
	USART_InitStructure.USART_Parity = USART_Parity_No;//无校验位
	USART_InitStructure.USART_HardwareFlowControl = USART_HardwareFlowControl_None;//无硬件数据流控制
	USART_InitStructure.USART_Mode = USART_Mode_Rx | USART_Mode_Tx;//配置收发模式
	USART_Init(USART3, &USART_InitStructure);
	
	USART_ITConfig(USART3, USART_IT_RXNE, ENABLE);//开启中断
	USART_Cmd(USART3, ENABLE);                    //使能串口 
}

• 串口中断接收数据 USART3_IRQHandler（）——控制小车状态

//定义标志位
u8 Fore,Back,Left,Right;

void USART3_IRQHandler(void) 
{
	int Bluetooth_data;
	if(USART_GetITStatus(USART3,USART_IT_RXNE)==SET)//RXNE置1表示数据达到DR寄存器
	{
		
		Bluetooth_data=USART_ReceiveData(USART3);//保存接收数据寄存器里面的数据
	
		//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=1,Right=0;//左
		//else if(Bluetooth_data==0x07)Fore=0,Back=0,Left=0,Right=1;//右
		//else if(Bluetooth_data==0x09)Mode_flag=1;//蓝牙控制模式
		//else if(Bluetooth_data==0x11)Mode_flag=2;//循迹控制模式
		//else if(Bluetooth_data==0x13)Mode_flag=3;//避障模式
		//else Fore=0,Back=0,Left=0,Right=0,Mode_flag=0;//刹

		if(Bluetooth_data=='Z')Fore=0,Back=0,Left=0,Right=0;//刹
		else if(Bluetooth_data=='A')Fore=1,Back=0,Left=0,Right=0;//前
		else if(Bluetooth_data=='E')Fore=0,Back=1,Left=0,Right=0;//后
		else if(Bluetooth_data=='C')Fore=0,Back=0,Left=1,Right=0;//左
		else if(Bluetooth_data=='G')Fore=0,Back=0,Left=0,Right=1;//右
		else if(Bluetooth_data=='L'){
			Mode_flag=1;OLED_Clear();OLED_ShowCHinese(15,3,0);OLED_ShowCHinese(42,3,1);																					OLED_ShowCHinese(69,3,6);OLED_ShowCHinese(96,3,7);}//蓝牙控制模式
		else if(Bluetooth_data=='M'){
			Mode_flag=2;OLED_Clear();OLED_ShowCHinese(15,3,2);OLED_ShowCHinese(42,3,3);																						OLED_ShowCHinese(69,3,6);OLED_ShowCHinese(96,3,7);}//循迹控制模式
		else if(Bluetooth_data=='N'){
			Mode_flag=3;OLED_Clear();OLED_ShowCHinese(15,3,4);OLED_ShowCHinese(42,3,5);																					OLED_ShowCHinese(69,3,6);OLED_ShowCHinese(96,3,7);}//避障模式
		else Fore=0,Back=0,Left=0,Right=0,Mode_flag=0;//刹

	}
}

//返送一个字符
void USART3_Send_Data(char data)
{
	USART_SendData(USART3,data);
	while(USART_GetFlagStatus(USART3,USART_FLAG_TC)!=1);
}

//发送字符串，为了修改蓝牙名字
void USART3_Send_String(char *String)
{
	u16 len,j;
	
	len=strlen(String);
	for(j=0;j<len;j++)
	{
		USART3_Send_Data(*String++);
	}
}

参考博文：
STM32——USART串口

6. exti.c

• 中断引脚初始化——PB5（接MPU6050INT引脚输出端）

• MPU6050的INT引脚接PB5

/*
设置为上拉输入，根据MPU6050的使用手册可知当其成功采集到一次数据，它的INT引脚就会拉低，产生下降沿从而触发外部中断。
*/
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;
	GPIO_InitStruct.GPIO_Speed=GPIO_Speed_50MHz;
	GPIO_Init(GPIOB,&GPIO_InitStruct);	
	
	//EXTI主要设置2个函数GPIO_EXTILineConfig（在库函数的gpio.h中）和EXTI_Init
	GPIO_EXTILineConfig(GPIO_PortSourceGPIOB,GPIO_PinSource5);//GPIO_EXTILineConfig用于配置AFIO的数据选择器，选择中断引脚
	
	EXTI_InitStruct.EXTI_Line=EXTI_Line5;//选择EXTI的中断线路
	EXTI_InitStruct.EXTI_LineCmd=ENABLE;//开启中断
	EXTI_InitStruct.EXTI_Mode=EXTI_Mode_Interrupt;//选择中断响应模式或事件响应模式
	EXTI_InitStruct.EXTI_Trigger=EXTI_Trigger_Falling;//下降沿触发，所以要选择上拉输入避免干扰	
	EXTI_Init(&EXTI_InitStruct);
}

参考博文：
STM32——STM32中断系统与EXTI外部中断

7. mpu6050（移植）

正点原子MPU6050代码下载：MPU6050参考代码

移植正点原子.c和.h的文件，文件名如下，在keil中添加.c以及包含.h的头文件夹路径

• mpu6050.c
• mpuiic.c
• inv_mpu.c（MPU官方库，含DMP解码运算）
• inv_mpu_dmp_motion_driver.c（MPU官方库，含DMP解码运算）
• mpu6050.h
• mpuiic.h
• inv_mpu.h
• inv_mpu_dmp_motion_driver.h

移植后修改如下内容：

进入mpu6050.c文件，修改MPU_Set_Rate(50); 将采样频率设置为100，则周期为10ms

if(res==MPU_ADDR)//器件ID正确
	{
		MPU_Write_Byte(MPU_PWR_MGMT1_REG,0X01);	//设置CLKSEL,PLL X轴为参考
		MPU_Write_Byte(MPU_PWR_MGMT2_REG,0X00);	//加速度与陀螺仪都工作
		MPU_Set_Rate(100);						//设置采样率为50Hz
 	}else return 1;
	return 0;

进入inv_mpu6050.h文件，修改宏定义DEFAULT_MPU_HZ

#ifndef _INV_MPU_H_
#define _INV_MPU_H_
#include "stm32f10x.h"

//定义输出速度
#define DEFAULT_MPU_HZ  (100)		//100Hz

#define INV_X_GYRO      (0x40)
#define INV_Y_GYRO      (0x20)
#define INV_Z_GYRO      (0x10)
#define INV_XYZ_GYRO    (INV_X_GYRO | INV_Y_GYRO | INV_Z_GYRO)
#define INV_XYZ_ACCEL   (0x08)
#define INV_XYZ_COMPASS (0x01)

进入mpuiic.c文件，修改IIC初始化函数 void MPU_IIC_Init(void)，将要初始化的IO口改为PB3/PB4

void MPU_IIC_Init(void)
{					     
  GPIO_InitTypeDef  GPIO_InitStructure;
	
  RCC_APB2PeriphClockCmd(RCC_APB2Periph_GPIOB,ENABLE);//先使能外设IO PORTB时钟 
		
  GPIO_InitStructure.GPIO_Pin = GPIO_Pin_3|GPIO_Pin_4;	 // 端口配置
  GPIO_InitStructure.GPIO_Mode = GPIO_Mode_Out_PP; 		 //推挽输出
  GPIO_InitStructure.GPIO_Speed = GPIO_Speed_50MHz;		 //IO口速度为50MHz
  GPIO_Init(GPIOB, &GPIO_InitStructure);					 //根据设定参数初始化GPIO 
	
  GPIO_SetBits(GPIOB,GPIO_Pin_3|GPIO_Pin_4);						 //PB10,PB11 输出高	
 
}

进入mpuiic.h文件，修改下面几个宏定义为PB3/PB4

#define MPU_IIC_SCL    PBout(3) 		//SCL
#define MPU_IIC_SDA    PBout(4) 		//SDA	 
#define MPU_READ_SDA   PBin(4) 		//输入SDA 

进入mpu6050.h文件，修改MPU6050的IIC地址

//如果AD0脚(9脚)接地,IIC地址为0X68(不包含最低位).
//如果接V3.3,则IIC地址为0X69(不包含最低位).
#define MPU_ADDR				0X68

一般经过上面几个步骤就已经移植成功了，不过有时候会出现MPU6050的DMP模式初始化失败的情况，这时候可以进入inv_mpu.c文件的u8 mpu_dmp_init(void)函数，关掉自检失败的return

//mpu6050,dmp初始化
//返回值:0,正常
//    其他,失败
u8 mpu_dmp_init(void)
{
	u8 res=0;
	MPU_IIC_Init(); 	//初始化IIC总线
	if(mpu_init()==0)	//初始化MPU6050
	{	 
		res=mpu_set_sensors(INV_XYZ_GYRO|INV_XYZ_ACCEL);//设置所需要的传感器
		if(res)return 1; 
		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;
	return 0;
}

8. control.c

• 使用MPU6050内部DMP算法读取小车当前在三个方向上的角度、角速度、角加速度，作为PID控制函数的形参，读取函数如下：

  /*
  - 角度Pitch，Roll，Yaw
  - 角速度gyrox，gyroy，gyroz
  - 角加速度aacx，aacy，aacz
  
  	角度的测量采用互补滤波：由陀螺仪测量的角速积分求角度和加速度计测量的加速度反求角度融合而成
  	互补滤波的效果与卡尔曼滤波效果差不多
  */
  mpu_dmp_get_data(&Pitch,&Roll,&Yaw);			//角度（角速度积分就是角度）
  MPU_Get_Gyroscope(&gyrox,&gyroy,&gyroz);	//角速度（陀螺仪测量）
  MPU_Get_Accelerometer(&aacx,&aacy,&aacz);	//角加速度（加速度计测量）
  

• 平衡小车的控制任务可以分解成三个基本控制任务。

  • 1 、首先是控制小车的平衡， 控制小车的平衡可以通过控制平衡小车的两个车轮的正反转实现平衡小车可以在两个运动方向都可以获得产生回复力的加速度， 这样平衡任务就可以实现。对应直立环，直立环的作用：无外加干扰时，小车可以保持平衡

  • 2、 然后是控制小车的速度， 由以上分析可知小车的速度与倾角有着密不可分的关系， 所以小车的速度变化应该与倾角相对应。对应速度环，速度环作用：克服外加干扰，小车保持平衡的同时速度为0

  • 3 、最后是控制小车的方向， 控制小车的方向可以利用两个车轮之间的转速差进行控制，转向速度应该与速度差的大小成正比 。对应转向环，转向环的作用：保证小车能走直线和转向

8.1 直立环Vertical

• 直立环需要让平衡车始终稳定在直立的状态，只要角度一倾斜，立马PID就要起作用通过控制电机转速来达到角度始终在机械中值位置。所以就要求小车的反应速度要快(Kp)，并且要具有预测性，让其能够达到提前控制的目的（Kd）。综上分析，我们需要P项和D项而不需要I项。

• 理论就是小车往那边倒，直立环PID的输出就为一个向前的PWM占空比的值，车轮就往哪边开，才可以保持车子的平衡。如果小车往后倾斜，那我们直立环PID的输出就为一个向后的PWM占空比的值在这里为负值。在我们Load函数中进行正负的判断以及车轮方向的控制。

• 为什么不能只采用比例控制？根据倒置单摆的原理，若只收到回复力，则系统会自由震荡，若还收到阻尼力，系统会趋向静止，所以不仅需要在电机上施加和倾角成正比的回复力，还需要增加和角速度成正比的阻尼力，阻尼力与运动方向相反
  

• 角速度的积分是角度，角度的微分是角速度，通过DMP单元，可以直接得到角度微分后的角速度

• 控制原理图
  

/*********************
直立环PD控制器：Kp*Ek（小车倾角与平衡位置的差） + Kd*Ek_D（小车俯仰角的角速度）

入口参数：期望角度、真实角度、真实角速度
出口值：直立环输出
变量：
- Med为中值角度，就是我们硬件上我们不上电他能够保持平横时间最长所对应的角度。
- Angle为我们MPU6050采集到的角度Roll或者是Pitch 大家根据你的陀螺仪安装方向来配置即可。
- gyro_X为角度Roll或者是Pitch方向上角速度。

*********************/
int Vertical(float Med,float Angle,float gyro_X)
{
	int PWM_out;
	
	PWM_out=Vertical_Kp*(Angle-Med)+Vertical_Kd*(gyro_X-0);
	return PWM_out;
}

8.2 速度环Velocity

• 运用了串级PID控制，即直立环作为内环，速度环的输出作为直立环的输入。目的是在小车平衡的前提下，对速度进行控制能够更加的顺滑。小车平衡的效果也能更好更稳定。
  

  • 速度环输入：1.给定速度。2.速度反馈。

  • 速度环输出：角度值（直立环的期望速度输入）

  • 直立环输入：1.给定角度（速度环输出）。2.角度反馈

  • 直立环输出：PWM（直接控制小车）

• 串级PID公式演变：通过公式可知串级PID可以简单看做是直立环控制+速度环控制，也就是说可以独立调试，在调试的时候可以先调节直立环，再调节速度环

  a = Kp*Ek（小车俯仰角 - a1） + Kd*Ek_D（小车俯仰角的角速度）
  a1 = Kp1*Ek（小车左右轮速度之和和零之间差值）+Ki*Ek_S（小车速度差累加和）
  //将a1代入到a式子中可得 串级PID公式
  a = Kp*小车俯仰角+Kd*小车俯仰角的角速度- kp[Kp1*小车左右轮速度之和和零之间差值+Ki*小车速度差累加和] 
  

/*********************
速度环PI：Kp*Ek（小车左右轮速度之和和零之间差值）+Ki*Ek_S（小车速度差累加和）【编码器得到的是距离，但是在固定时间内读取清零后的数据可以近似当做速度】
入口参数为：
- Target——期望速度（若为0，小车将稳在原地，我们通过蓝牙去控制此值可以达到移动的效果）
- encoder_left——左车轮编码器读取到的速度
- encoder_right——右车轮编码器读取到的速度
出口值：速度环输出
*********************/
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;//大于0.5即可，为了放大上一次的偏差
	
	//1.计算速度偏差
	Encoder_Err=((encoder_left+encoder_right)-Target);
	//2.对速度偏差进行低通滤波，因为速度环对直立环是干扰
	//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;//低通滤波是为了防止速度过大的影响直立环的正常工作。在平衡控制中直立环控制是最主要的
	//3.对速度偏差积分，积分出位移
	Encoder_S+=EnC_Err_Lowout;
	//4.积分限幅
	Encoder_S=Encoder_S>10000?10000:(Encoder_S<(-10000)?(-10000):Encoder_S);
	
	//5.速度环控制输出计算
	PWM_out=Velocity_Kp*EnC_Err_Lowout+Velocity_Ki*Encoder_S;
	return PWM_out;
}

8.3 转向环Turn

• 当我们不转向时 RC=0 这时只有一个Turn_Kd在约束Z轴的角速度。效果是我们用手去旋转小车有一个抵抗的力。这样可以让我们的车在平衡时不打转，走直线时不会歪。
• 当我们转向时设置 Turn_Kd=0，转向时的约束为0，这时我们给RC赋值，若为正输出的PWM值为正，若为负输出pwm值为负。在最终加载到电机的PWM值左电机减右电机加即可形成差速从而达到转向的效果。

/*********************
转向环：Kp*EK（遥控数据）+Kd*EK_A（Z轴角速度）
入口参数：
- gyro_Z —— Z轴角速度
- RC——蓝牙接收的期望转向速度，通过轮胎的反馈速度和航向角Yaw来给定差速。比例系数计算控制理想差速度（设置为0，小车理想状态是处于静止平衡状态）和实际差速度的差值（Turn_Target）
*********************/
int Turn(int gyro_Z,int RC)
{
	int PWM_out;
	//这不是一个严格的PD控制器，Kd针对的是转向的约束，但Kp针对的是遥控的转向。
	PWM_out=Turn_Kd*gyro_Z + Turn_Kp*RC;
	return PWM_out;
}

8.4 中断函数（10ms）——PID计算，限幅，加载到电机

• MPU6050的DMP控制器最大频率为200HZ，5ms就能输出一次数据，那么我们可以采用定时器5ms中断一次，在中断函数中处理。也可以采用外部中断，也就是接MPU6050的INT引脚，进入中断函数也可以再延时5ms，也就是10ms进行一次姿态调节，也可以直接修改DMP源文件的采样频率。
• 将DMP读取和PID算法同时放到外部中断当中，用MPU6050自带的中断引脚INT发出的信号作为定时信息。这样就能维持高度的周期性，且PID和DMP也会高度同步。原因是，PID算法需要保持良好的周期性。特别是在进行PD运算时，短暂的时差就会导致巨大的误差。若将读取DMP数据的函数和PID算法都放到main中执行，MCU的每个运行周期受到运算量的影响，所以PD算法也会有随机误差产生。
• 选择简单的串级PID算法，所谓串级PID类似于串联，它将PD直立环（balance）、PI速度环（velocity）和PD转向环（turn）结果分别计算最后相加。得到输出左右轮的PWMA和PWMB输出。其极性代表转向。公式如下：
  

float Pitch,Roll,Yaw;			
short gyrox,gyroy,gyroz;	
short aacx,aacy,aacz;	
int Encoder_Left,Encoder_Right;

void EXTI9_5_IRQHandler(void)
{
	int PWM_out;
	if(EXTI_GetITStatus(EXTI_Line5)!=0)//一级判定
	{
		if(PBin(5)==0)//二级判定
		{
			EXTI_ClearITPendingBit(EXTI_Line5);//清除中断标志位
			
			//1.采集编码器数据&MPU6050角度信息。
			Encoder_Left=-Read_Speed(2);//电机是相对安装，刚好相差180度，为了编码器输出极性一致，就需要对其中一个取反。
			Encoder_Right=Read_Speed(4);
			
			//DMP读取
	 		mpu_dmp_get_data(&Pitch,&Roll,&Yaw);			//角度
			MPU_Get_Gyroscope(&gyrox,&gyroy,&gyroz);	//陀螺仪--角速度
			MPU_Get_Accelerometer(&aacx,&aacy,&aacz);	//加速度
			

 	if(Mode_flag==3)  //跟随模式
	{	 
		UltrasonicWave_StartMeasure();
		
	 	if(UltrasonicWave_Distance<=70)Target_Speed=10;
		else if(UltrasonicWave_Distance>100&&UltrasonicWave_Distance<=200)Target_Speed=-10;
		else Target_Speed=0;
	}	
    if(Mode_flag==2)//循迹模式
    {	
    	Tracking_detection();
    }
	else if(Mode_flag==1)//蓝牙模式，通过蓝牙控制标志位达到控制的效果
	{		/*前后*/
			if((Fore==0)&&(Back==0))Target_Speed=0;//未接受到前进后退指令-->速度清零，稳在原地
			if(Fore==1)Target_Speed=-25;// 前进1标志位拉高-->需要前进
			if(Back==1)Target_Speed=25;//
			Target_Speed=Target_Speed>SPEED_X?SPEED_X:(Target_Speed<-SPEED_X?(-SPEED_X):Target_Speed);//限幅
			
			/*左右*/
			if((Left==0)&&(Right==0))Turn_Speed=0;
			if(Left==1)Turn_Speed-=50;	//左转
			if(Right==1)Turn_Speed+=50;	//右转
			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.7;//若无左右转向指令，则开启转向约束
			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,Turn_Speed);								//转向环												
			

			PWM_out=Vertical_out;//最终输出，Vertical_out是串联速度环和直立环
			//3.把控制输出量加载到电机上，完成最终的的控制。
			MOTO1=PWM_out-Turn_out;//左电机，串联三个环的输出
			MOTO2=PWM_out+Turn_out;//右电机，串联三个环的输出
			Limit(&MOTO1,&MOTO2);	 //PWM限幅			
			Load(MOTO1,MOTO2);		 //加载到电机上。
			
//			Stop(&Med_Angle,&Roll);
			
		}
	}
}

其中的参数统一使用宏定义或者全局变量。

/*
0x00：刹车
0x01：前进
0x05：后退
0x03：左转
0x07：右转
*/

u8 Mode_flag;					//模式选择标志位
float Med_Angle=-12;	//机械中值。
float Target_Speed=0;	//期望速度（俯仰）。---用于控制小车前进后退及其速度。
float Turn_Speed=0;		//期望速度（偏航）

float 
	Vertical_Kp=-630//420*0.6=252     770*0.6=462   -1300*0.6=-780    -870  -1050*0.6
	,//直立环KP、KD
	Vertical_Kd=-1.85;    //1*0.6     2.3*0.6=1.38  -3*0.6=-1.8   -4.2*0.6  -3*0.6
float 
	Velocity_Kp=0.24,//速度环KP、KI  0.21   0.45
	Velocity_Ki=0.0012;         0.0015   0.004
float 
	Turn_Kd=-0.7,//转向环KP、KD -0.7
	Turn_Kp=20;//20

int Vertical_out,Velocity_out,Turn_out;//直立环&速度环&转向环 的输出变量

//PID控制函数声明
int Vertical(float Med,float Angle,float gyro_Y);//函数声明
int Velocity(int Target,int encoder_left,int encoder_right);
int Turn(int gyro_Z,int RC); 

//void chaoshengbo(void);

#define SPEED_X 30 //俯仰(前后)最大设定速度
#define SPEED_Z 100//偏航(左右)最大设定速度 

9. sys.c

NVIC配置(中断优先级)

外部中断PID运算的优先级必须是最高的所以在这里取0,0。

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);
	
	//串口
	NVIC_InitStruct.NVIC_IRQChannel=USART1_IRQn;
	NVIC_InitStruct.NVIC_IRQChannelCmd=ENABLE;
	NVIC_InitStruct.NVIC_IRQChannelPreemptionPriority=2;
	NVIC_InitStruct.NVIC_IRQChannelSubPriority=2;
	NVIC_Init(&NVIC_InitStruct);	
	
	//蓝牙
	NVIC_InitStruct.NVIC_IRQChannel=USART3_IRQn;
	NVIC_InitStruct.NVIC_IRQChannelCmd=ENABLE;
	NVIC_InitStruct.NVIC_IRQChannelPreemptionPriority=2;
	NVIC_InitStruct.NVIC_IRQChannelSubPriority=2;
	NVIC_Init(&NVIC_InitStruct);	
												
}

四、PID算法参数整定

• 对于串级PID，一般先调节直立环，再调节速度环，在调节完这两个环节之后小车就能很好保持平衡且移动后会回到原来的位置，但是走直线还不够直。最后我们调节转向环，用于调节转向和走直线。

1. 机械中值寻找步骤

1. 在OLED显示屏上显示对应的角度或者通过串口实时反馈角度。
2. 观察角度，一只手把平衡从一个方向往另一个方向托，在其快速倒下的瞬间我们记住当前的角度值。另一个方向同理，我们将两个角度值相加除以2得到的中间值作为机械中值。（这个值往往不是0度可能是-1°、-2°、-3°或1°、2°、3°这是由小车的硬件所决定的）

2. 直立环调节

• 角度增益决定了反馈的线性倍数，直接反映就是小车的调节幅度，P1太小，小车反馈影响小；P1太大，小车反馈过度，会出现大幅度的震荡；调节时第一步要把其他五个参数全部归零，P从小到大（约1~200）调节，直到调整到某一个值，小车能够较大幅度地来回震荡，说明P1的值已经能够把小车修正回来了，此时可以再稍微加大一点P1，到此，小车出现了明显的超调，下一步就是解决超调问题。

A.Kp的极性和大小的确定

• 首先我们确定Kp的极性，我们将小车的Kp设置为200。如果小车向那边倒车轮就往反方向加速转，这是我们不希望看到的正反馈。这时我们要修改Kp的极性，反之说明极性正确，这里我们的Kp极性确定为负值。

• 确定Kp大小的方法是将其一直增加，直到出现大幅度的低频抖动，大致过程如下：

  • 设定Kp=-30, 小车虽然有平衡的趋势，但是显然响应太慢了。
  • 设定Kp=-100, 可以看到响应有所加快，但是松手后小车依然会快速倒下。
  • 设定Kp=-160,小车的响应明显加快，而且来回推动小车的时候，会有大幅度的低频抖动。说明这个时-候Kp值已经足够大了，需要增加微分控制削弱比例控制，抑制低频抖动。

B.Kd的极性和大小的确定

• 将Kp的值设定为0，再设定Kd=0.5。当我们拿起小车旋转的时候，车轮会反向转动，并没有能够实现跟随效果。这说明了Kd的极性反了。接下来，我们设定Kd=-0.5,这个时候我们可以看到，当我们旋转小车的时候，车轮会同向以相同的速度跟随转动，这说明我们实现了角速度闭环，至此，我们可以确定kd 的极性是负的。

• 打开确定好的比例控制Kp，将Kd从小到大增加，直到小车出现剧烈高频抖动。

  • 设定Kd=-0.5, 这时由于比例控制所导致的低频抖动已经基本消失。（在系统出现抖动之前，Kd参数越大越好，响应越快，所以Kd增大）
  • 设定Kd=-1, 可以看到小车已经趋于平衡，且松开手后可以保持短暂平衡。
  • 设定Kd=-2.3, 小车开始出现高频剧烈抖动，但是在遇到这种情况时需要立刻关闭小车，否则长时间高频抖动会导致驱动被烧坏的。

• 按照前人的调试经验，这时我们已经获得了最大的比例和微分数值。这时，将我们这个数值乘以0.6就是我们需要的数值，最后通过微调得到：Kd=-97, Kd=-1.38。

3. 速度环调节

• 速度环，与角度环的目标是角度为零不同，速度控制的目标是让小车速度为零。I会对速度进行积分，如果小车向某个方向长时间有一定速度的话，I会产生相反的越来越大的反馈从而让小车及时停下加速的举动。I也会对突然的加速比如人手突然推一下小车做出快速的响应，有助于小车稳定；在PD的基础上，I从小到大增加（0.05~0.3）。一般来说，I在比较小时便可看到小车明显有改善，I在保证小车稳定的情况下应当尽可能小一点。

• 积分项由偏差的积分得到，所以Kp和Ki的极性相同，且是一种线性控制器，它们的系数大小一般有Ki=Kp/200的关系。

• 虽然这里的 PI 控制器也是速度控制常用的一种控制，但是和普通的调速系统不一样，这里的速度控制是正反馈的，当小车以一定的速度运行的时候，我们要让小车停下来，小车需要行驶更快的速度去“追”，小车运行的速度越快，去“追”的速度也就越快，所以这是一个正反馈的效果。如果使用常规的速度负反馈，当小车以一定的速度运行的时候，我们通过减速让小车慢下来，小车会因为惯性向前倒下

A．确定Kp,Ki的极性

• 关闭直立环控制和转向控制后，设置Kp=100,Ki=0.5，拿起小车将其中一个轮胎向任意一个方向旋转，另一个轮胎会自动反向旋转。这就是一般控制系统中的负反馈现象。

• 设置Kp=-100,Ki=-0.5，拿起小车将其中一个轮胎向任意一个方向旋转，另一个轮胎会向相同的方向旋转并逐渐加速到最大值，正反馈现象。自此，我们可以确定Kp,Ki的极性为负值。

B,确定Kp,Ki的大小

• 由于直立环和速度环是相互制约的系统，所以先打开已经调整好的直立环控制。增加KpKi直至小车速度为0，且回位效果好

  • 设定 Kp=-100,Ki=-0.5，此时小车的速度控制比较弱，很难让速度恒定。
  • 设定 Kp=-150,Ki=-0.75，此时小车已经可以保持直立，但是轻推小车后，小车会向一个方向“滑行”很长一段距离，说明对于速度的控制依然不够。
  • 设定 Kp=-200,Ki=-1，此时小车已经可以保持直立，但是轻推小车后，小车会剧烈抖动，说明对速度的控制过大，抗干扰能力弱。

• 通过以上步骤，我们可以判断Kp, Ki的范围为Kp:-150 ~ -200,Ki:-0.75 ~ -1，通过反复调整得到合适的参数

4. 转向环调节

• 角度微分D是对角度的变化率进行反馈，就是小车角度往哪个方向变化，D就要减小这种变化，从而让小车趋于稳定；D太小，达不到反馈的效果，小车仍然有明显的震荡；D太大，小车对角度的细小变化过于敏感，可能会出现高频振动；D从小到大增加（0.01~0.8），直到某一值时，小车能短暂平衡约5秒钟才倒下，并且也不会出现高频振动，D也就调整好了。但是此时小车会先平衡后沿着某个方向慢慢运动直到加速失衡，这是因为到目前为止我们只是通过PD角度环使得角度基本稳定在0°，但是小车速度并没有控制，所以微小的角度误差不断累积，最终变成小车向某个方向缓慢不断加速直至电机到达极限倒下。

• 想要小车走直线或者转向还需要转向环控制转向角和两轮的差速度值。这里我们使用左右轮编码器数据之差的积分值作为比例控制量，以 Z 轴陀螺仪作为微分控制的输入进行 PD控制，目标是保持转向角为设定值。

A.确定Kp, Ki的极性

• 将直立环和速度环关闭，设置Kp=-10,Ki=-0.1，将小车放到地面尝试绕着Z轴转动小车。发现小车能够很轻易地被转动，这样的正反馈是错误的。设置Kp=10,Ki=0.1重复上述操作，我们发现小车很难被转动，说明极性正确，可以确定Kp, Ki均为正值。

B.确定Kp, Ki的大小

• 首先开启已经调试好的转向换和速度环。增大Kp，直到走直线很好且抖动很小

  • 设置Kp=10, Ki=0.1，控制小车前进，发现小车走直线的偏差很大。
  • 设置Kp=30, Ki=0.3，控制小车前进，小车走直线的能力已经很好。
  • 设置Kp=70, Ki=0.7，控制小车前进，小车在急停的时候又剧烈抖动，对方向的控制过大。

• 可以确定Kp:30~ 70, Ki:0.3~0.7, 通过多次调整确定Kp和Ki。最后小车具有很好的平衡特性，且能完成前进后退以及转向的功能。

STM32平衡车入门PID —— 第三天（PID调参）

五、程序执行流程

代码