【STM32+HAL】二轮平衡小车

一、前言

偶然获得轮趣的二轮平衡小车一套，翻阅官方资料感觉内容较为杂糅，故写下此文旨在实现简洁的二轮平衡功能。

二、软件工具

1、芯片： STM32F407ZGT6

2、IDE： MDK-Keil软件

3、库文件：STM32F4xxHAL库

三、基本思路

MPU6050陀螺仪每5ms检测当前偏移角度，目标角度为0度，此为PID的控制内环，平衡环。

编码器中断每5ms读取当前马达编码值，目标速度为0，此为PID控制外环，位置环。

最后，根据两个环计算相加量，每隔10ms发出控制指令控制左右马达一起运动，达到最终平衡目的——MPU6050保持水平，小车保持原地静止。

四、CubeMX配置

1、配置PWM波定时器TIM2

频率 frequency = 84MHz / 1 / 8400 = 10kHz

2、配置左右马达编码器TIM3\4

Encoder Mode Tl1 and Tl2：四分频模式，精度更高。有关编码器的更多内容，详见【STM32+HAL】直流电机PID控制

Input Filter：输入滤波，平滑曲线

3、配置10ms控制定时器TIM1

4、配置5ms读取角度及编码值定时器TIM6

5、配置IIC外设与MPU6050通信

建议将引脚配置为上拉模式，并选择快速模式，使通信更稳定。

至此，CubeMX配置完毕。

五、Keil填写代码

1、MPU6050

有关MPU6050读取角度值、四元数求解以及滤波操作，详见【STM32+HAL】姿态传感器陀螺仪MPU6050模块

这里仅附读取角度的代码，其余代码基本一致。

/**************************************************************************
Function: Get angle
Input   : way：The algorithm of getting angle 1：DMP  2：kalman  3：Complementary filtering
Output  : none
函数功能：获取角度
入口参数：无
返回  值：无
**************************************************************************/
void Get_Angle(void)
{
	MpuGetData();							//读取加速度、角速度、倾角
	GetAngle(&MPU6050,&Angle,0.003f);
	Angle_Balance = Roll;             	 	//更新平衡倾角,前倾为正，后倾为负
	Gyro_Balance = gyro0;              		//更新平衡角速度,前倾为正，后倾为负
//	printf("%.1f,%.1f\r\n",Angle_Balance, Gyro_Balance);
}

2、读取编码值

马达编码值的具象表现为定时器的计数值，每当读取一次后，就需要对其进行清零操作，以保证下次读数的准确性。

可以把编码值想象为马达转过的距离，编码值越大，转过的距离越远。此时设置定时一小段时间读取这段时间走过的距离值，即可将此段距离值等效为速度值。

当然，这个数值不是马达的真实转速，不过在程序里我们只需要一个线性量，所以可以简单处理。

/**************************************************************************
Function: Read encoder count per unit time
Input   : TIMX：Timer
Output  : none
函数功能：单位时间读取编码器计数
入口参数：TIMX：定时器
返回  值：速度值
**************************************************************************/
int Read_Encoder(u8 TIMX)
{
	int Encoder_TIM;
	switch(TIMX)
	{
		case 3:  Encoder_TIM = (short)TIM3 -> CNT;  TIM3 -> CNT = 0;break;
		case 4:  Encoder_TIM = (short)TIM4 -> CNT;  TIM4 -> CNT = 0;break;
		default: Encoder_TIM = 0;
	}
	return Encoder_TIM;
}

3、平衡环

Middle_angle为MPU6050保持水平时程序返回的当前角度值，此值未必为0，需要自己测量。

将此函数计算出来的数值赋值给定时器的CCR后，即可实现小车，但此时小车不会静止在一个位置，会朝着一个方向做匀速直线运动，所以我们需要再加上位置环，使其在一个位置实现平衡。

/**************************************************************************
Function: Vertical PD control
Input   : Angle:angle；Gyro：angular velocity
Output  : none
函数功能：平衡环、速度环PD控制
入口参数：Angle:角度；Gyro：角速度
返回  值：无
**************************************************************************/
int Balance(float Angle,float Gyro)
{
	float Balance_Kp = 100, Balance_Kd = 2;							//直立环PD参数
	float Angle_bias, Gyro_bias;
	int balance;
	Angle_bias = Middle_angle - Angle;                       		//求出平衡的角度中值 和机械相关
	Gyro_bias = 0 - Gyro;
	balance = Balance_Kp * Angle_bias + Balance_Kd * Gyro_bias;   	//计算平衡控制的电机PWM
	return balance;
}

4、位置环

稳定在一个位置的本质即左右马达的速度为0，故这里的位置环也即速度环。

代码不难，只加了几个滤波和积分限幅，大家自行理解吧！

/**************************************************************************
Function: Speed PI control
Input   : encoder_left：Left wheel encoder reading；encoder_right：Right wheel encoder reading
Output  : Speed control PWM
函数功能：速度控制PWM


入口参数：encoder_left：左轮编码器读数；encoder_right：右轮编码器读数
返回  值：速度控制PWM
**************************************************************************/
int Velocity(int encoder_left,int encoder_right)
{
	float Velocity_Kp = 5, Velocity_Ki = 1.8;										//速度环PI参数
	static float velocity, Encoder_Least, Encoder_bias;
	static float Encoder_Integral;
	if(Flag_Stop == 1)   Encoder_Integral = 0;      								//电机关闭后清除积分
	
	/*=================速度PI控制器===================*/
	Encoder_bias *= 0.8;		                                          			//一阶低通滤波器
	Encoder_Least = 0 - (encoder_left + encoder_right);                    			//获取最新速度偏差==测量速度（左右编码器之和）-目标速度（此处为零）
	Encoder_bias += Encoder_Least * 0.2;											//上次偏差的0.8 + 本次偏差的0.2，减缓速度差值，减少对直立的干扰

	Encoder_Integral += Encoder_bias;                                  				//积分出位移 积分时间：10ms
	if(Encoder_Integral > 10000)	Encoder_Integral = 10000;             			//积分限幅
	if(Encoder_Integral < -10000)	Encoder_Integral = -10000;            			//积分限幅
	velocity = Encoder_bias * Velocity_Kp + Encoder_Integral * Velocity_Ki;        	//速度控制
	return velocity;
}

5、赋值

将两环计算出来的数值相加就是所求数值。

赋值函数中的宏定义为控制马达转动方向引脚：

AIN11，AIN20——AIN1为高电平，AIN2为低电平，此时左马达正转；

AIN10，AIN21——AIN1为低电平，AIN2为高电平，此时左马达反转 ... ... 其余同理

#define AIN11	HAL_GPIO_WritePin(AIN1_GPIO_Port,AIN1_Pin,GPIO_PIN_SET);
#define AIN10	HAL_GPIO_WritePin(AIN1_GPIO_Port,AIN1_Pin,GPIO_PIN_RESET);
#define AIN21	HAL_GPIO_WritePin(AIN2_GPIO_Port,AIN2_Pin,GPIO_PIN_SET);
#define AIN20	HAL_GPIO_WritePin(AIN2_GPIO_Port,AIN2_Pin,GPIO_PIN_RESET);
#define BIN11	HAL_GPIO_WritePin(BIN1_GPIO_Port,BIN1_Pin,GPIO_PIN_SET);
#define BIN10	HAL_GPIO_WritePin(BIN1_GPIO_Port,BIN1_Pin,GPIO_PIN_RESET);
#define BIN21	HAL_GPIO_WritePin(BIN2_GPIO_Port,BIN2_Pin,GPIO_PIN_SET);
#define BIN20	HAL_GPIO_WritePin(BIN2_GPIO_Port,BIN2_Pin,GPIO_PIN_RESET);

/**************************************************************************
Function: Control function
Input   : none
Output  : none
函数功能：所有的控制代码都在这里面
入口参数：无
返回  值：无
**************************************************************************/
void Control(void)
{
	int Balance_Pwm, Velocity_Pwm = 0;				//平衡环PWM变量，速度环PWM变量
	int Motor_Left, Motor_Right;      				//位置环PWM变量

	Balance_Pwm = Balance(Angle_Balance, Gyro_Balance); //平衡PID控制	  Gyro_Balance平衡角速度极性：前倾为正，后倾为负
//	Velocity_Pwm = Velocity(Encoder_Left, Encoder_Right);//速度环PID控制
	/* 记住，速度反馈是正反馈，就是小车快的时候要慢下来就需要再跑快一点 */

	Motor_Left  = PWM_Limit(Balance_Pwm + Velocity_Pwm, Limit, -Limit);
	Motor_Right = PWM_Limit(Balance_Pwm + Velocity_Pwm, Limit, -Limit);		//PWM限幅

	Set_Pwm(Motor_Left, Motor_Right);
}



/**************************************************************************
Function: Assign to PWM register
Input   : motor_left：Left wheel PWM；motor_right：Right wheel PWM
Output  : none
函数功能：赋值给PWM寄存器
入口参数：左轮PWM、右轮PWM
返回  值：无
**************************************************************************/
void Set_Pwm(int motor_left,int motor_right)
{
	if(motor_left > 0)	//前进
	{
		AIN11;
		AIN20;
	}
	else				//后退
	{
		AIN10;
		AIN21;
	}
	PWMA = myabs(motor_left);

	if(motor_right > 0) //前进
	{
		BIN11;
		BIN20;
	}
	else				//后退
	{
		BIN10;
		BIN21;
	}
	PWMB = myabs(motor_right);
}

6、定时器回调函数

这里的按键控制程序的开启与暂停。

/* USER CODE BEGIN 4 */
void HAL_TIM_PeriodElapsedCallback(TIM_HandleTypeDef *htim){
	
	/* MPU6050读取 */
	if(htim -> Instance == TIM6){
		Get_Angle();                            	//更新姿态	5ms读取一次
		Encoder_Left = Read_Encoder(3);          	//读取左轮编码器的值，前进为正，后退为负
		Encoder_Right = Read_Encoder(4);         	//读取右轮编码器的值，前进为正，后退为负
	}
	
	/* 控制定时器 */
	if (htim -> Instance == TIM1){
		Control();
	}
}

void HAL_GPIO_EXTI_Callback(uint16_t GPIO_Pin)  
{
	if(HAL_GPIO_ReadPin(WKUP_GPIO_Port,WKUP_Pin) == GPIO_PIN_SET){
		HAL_Delay(20); //延时消抖
		if(HAL_GPIO_ReadPin(WKUP_GPIO_Port,WKUP_Pin) == GPIO_PIN_SET){
			HAL_GPIO_TogglePin(LED_GPIO_Port,LED_Pin);
			Flag_Stop = !Flag_Stop;
		}
	}
}
/* USER CODE END 4 */

7、初始化程序

最后，记得对程序进行初始化。

  /* USER CODE BEGIN 2 */
  
	/* MPU6050初始化 */
	MPU_Init();
	HAL_TIM_Base_Start_IT(&htim6);
	HAL_Delay(500);
	
	/* PWM初始化 */
	HAL_TIM_Encoder_Start(&htim3, TIM_CHANNEL_ALL);
	HAL_TIM_Encoder_Start(&htim4, TIM_CHANNEL_ALL);
	HAL_TIM_PWM_Start(&htim2, TIM_CHANNEL_1);
	HAL_TIM_PWM_Start(&htim2, TIM_CHANNEL_2);
	
	HAL_TIM_Base_Start_IT(&htim1);
  /* USER CODE END 2 */

六、程序源码

夸克网盘：我用夸克网盘分享了「Balance_Wheele」提取码：cuMG

百度网盘：通过百度网盘分享的文件：Balance_Wheele 提取码：6666

GItee：Balance_Wheele

CSDN：Balance-Wheele

七、结语

本人能力有限，代码未必是最优解，若有可改进之处望在评论区留言。

如有小伙伴想交流学习心得，欢迎加入群聊751950234，群内不定期更新代码，以及提供本人博客所有源码