超详细！STM32平衡小车控制核心-直立环与速度环

  

一、代码介绍

0. 保护小车

当倾斜角度大于某一值时，我们关闭电机，防止小车摔倒后，电机仍处于较大转速。电机关闭时，返回值为0，正常时为1。

u8 Turn_Off(float angle)
{
	u8 temp;
	if(angle<-40||angle>40)//倾角大于40度关闭电机
	{	                                            
		temp=0;                                         
		Set_Pwm(Left_moter,0);  //关闭左电机
		Set_Pwm(Right_moter,0); //关闭右电机
	}
	else
		temp=1;
	return temp;			
}

1. 直立环

   直立环中机械中值的确定：在小车进行两边分别倾斜时的临界值相加除2，来确定。或者通过多次调试来确定更改。这个机械中值很重要，是确保平衡的重要因素。

#define Middle_angle 2.8  //直立环的机械中值

typedef struct PID {
		float  Kp;         //  Proportional Const  P系数
		float  Ki;           //  Integral Const      I系数
		float  Kd;         //  Derivative Const    D系数
		
		float  PrevError ;          //  Error[-2]  
		float  LastError;          //  Error[-1]  
		float  Error;              //  Error[0 ]  
		float  DError;            //pid->Error - pid->LastError	
		float  SumError;           //  Sums of Errors  
		
		float  output;
		
		float  Integralmax;      //积分项的最大值
		float  outputmax;        //输出项的最大值
} PID;


//直立环
float PID_Balance_Calc(PID *pid, float Angle,float Gyro)  
{  
   float Angle_bias,Gyro_bias;
	 Angle_bias=Middle_angle-Angle;                    				//求出平衡的角度中值 和机械相关
	 Gyro_bias=0-Gyro; 
	 pid->output= -pid->Kp/100*Angle_bias-Gyro_bias*pid->Kd/100; //计算平衡控制的电机PWM  PD控制   kp是P系数 kd是D系数 
	
	if(pid->output > pid->outputmax )    pid->output = pid->outputmax;
	if(pid->output < - pid->outputmax )  pid->output = -pid->outputmax;
	
	return pid->output;
}

2. 速度环

//为了防止积分项过度累积，引入积分项的限幅是一种常见的做法。
//限制积分项的幅值可以防止积分项过度增加，从而限制了系统的累积误差。这样可以避免系统过度响应或者不稳定。
float abs_limit(float value, float ABS_MAX)   //积分限幅，设置最大值。
{
	if(value > ABS_MAX)
		value = ABS_MAX;

	if(value< -ABS_MAX)
		value = -ABS_MAX;
	return value;
}

float PID_Speed_Calc(PID *pid, int encoder_left, int encoder_right)  
{  
    static float Encoder_bias;
	pid->Error = 0-(encoder_left+encoder_right);    //获取最新速度偏差==测量速度（左右编码器之和）-目标速度（此处为零）             
    Encoder_bias *=0.8;    //一阶低通滤波器     
    Encoder_bias += pid->Error*0.2;  //一阶低通滤波器  
    pid->SumError +=Encoder_bias;
    
	pid->output  = -pid->Kp* Encoder_bias 
	               -pid->Ki* abs_limit( pid->SumError, 10000);

	if(Turn_Off(Angle_Balance)==1)   pid->SumError=0;      //电机关闭后清除积分	
	
	if(pid->output > pid->outputmax )    pid->output = pid->outputmax;
	if(pid->output < - pid->outputmax )  pid->output = -pid->outputmax;						
	return pid->output ;   //输出为pwm值
}

3. 获取角度函数

这里使用DMP库进行获取角度。点击：移植DMP库详情介绍。

void Get_Angle(void)
{ 
	Read_DMP();                      	 //读取加速度、角速度、倾角
	Angle_Balance=Pitch;             	 //更新平衡倾角,前倾为正，后倾为负
	Gyro_Balance=gyro[0];              //更新平衡角速度,前倾为正，后倾为负
}

4. 输出限幅函数

int PWM_Limit(int IN,int max,int min)
{
	int OUT = IN;
	if(OUT>max) OUT = max;
	if(OUT<min) OUT = min;
	return OUT;
}

5. Mpu6050外部中断

   MPU6050的INT引脚就是用来触发外部中断的，当MPU6050数据发生更新后，该引脚会置低。所以我们可以不使用定时器来进行平衡车的控制，而是通过外部中断来控制小车。这里我们使用PC9来设置为中断触发引脚。
点击这里：STM32外部中断详情介绍地址。

#include "exit.h"
#include "tim4.h"
#include "tim1.h"
#include "tim2.h"
#include "direction.h" 
#include "pid.h"

PID Speed_PID;  //速度环
PID Balance_PID;  //直立环

void MiniBalance_EXTI_Init(void)
{  
		GPIO_InitTypeDef GPIO_InitStructure;
		EXTI_InitTypeDef EXTI_InitStructure;
		NVIC_InitTypeDef NVIC_InitStructure;
		
		RCC_APB2PeriphClockCmd(RCC_APB2Periph_AFIO,ENABLE);   //外部中断，需要使能AFIO时钟
		RCC_APB2PeriphClockCmd(RCC_APB2Periph_GPIOC, ENABLE); //使能GPIO端口时钟
		
		GPIO_InitStructure.GPIO_Pin = GPIO_Pin_9;	          //端口配置
		GPIO_InitStructure.GPIO_Mode = GPIO_Mode_IPU;         //上拉输入
		GPIO_Init(GPIOC, &GPIO_InitStructure);					      //根据设定参数初始化GPIO
		GPIO_EXTILineConfig(GPIO_PortSourceGPIOC,GPIO_PinSource9);
		
		EXTI_InitStructure.EXTI_Line=EXTI_Line9;
		EXTI_InitStructure.EXTI_Mode = EXTI_Mode_Interrupt;	
		EXTI_InitStructure.EXTI_Trigger = EXTI_Trigger_Falling;//下降沿触发
		EXTI_InitStructure.EXTI_LineCmd = ENABLE;
		EXTI_Init(&EXTI_InitStructure);	 	//根据EXTI_InitStruct中指定的参数初始化外设EXTI寄存器
		
		NVIC_InitStructure.NVIC_IRQChannel = EXTI9_5_IRQn;			//使能按键所在的外部中断通道
		NVIC_InitStructure.NVIC_IRQChannelPreemptionPriority = 0x02;	//抢占优先级2， 
		NVIC_InitStructure.NVIC_IRQChannelSubPriority = 0x01;					//子优先级1
		NVIC_InitStructure.NVIC_IRQChannelCmd = ENABLE;								//使能外部中断通道
		NVIC_Init(&NVIC_InitStructure); 
}


int EXTI9_5_IRQHandler(void) 
{    
	int Motor_Left,Motor_Right;      //电机PWM变量 应是Motor的 向Moto致敬	
	if(INT==0)		   //当陀螺仪的数据变化时，该引脚为低
	{   
		EXTI_ClearITPendingBit(EXTI_Line9); //清除标志位
		Get_Angle();  //获取角度
		if(Turn_Off(Angle_Balance))  //倾斜角度正常时
		{
			Encoder_Left  = -TIM4_ReadEncoder();  //获取编码器值
			Encoder_Right =  TIM1_ReadEncoder(); 
			
			Balance_Pwm = PID_Balance_Calc(&Balance_PID, Angle_Balance, Gyro_Balance);
			Velocity_Pwm=PID_Speed_Calc(&Speed_PID, Encoder_Left, Encoder_Right);
			//Velocity_Pwm= PID_Incremental_Calc(&Speed_PID, 0,  Balance_Pwm);
			//	Velocity_Pwm=Velocity(Encoder_Left,Encoder_Right);
			//	Turn_Pwm=Turn(Gyro_Turn);		
			Motor_Left= Velocity_Pwm+Balance_Pwm;
			Motor_Right=Velocity_Pwm+Balance_Pwm;
			
			Motor_Left=PWM_Limit(Motor_Left,3000,-3000);
			Motor_Right=PWM_Limit(Motor_Right,3000,-3000);
			
			Set_Pwm(Left_moter,-Motor_Left);  //3.PWM输出给电机
			Set_Pwm(Right_moter,-Motor_Left);  //3.PWM输出给电机 
		}
	}
	 return 0;	 
}

二、调试介绍

在调试过程中，我们需要单个将某个环调好后，再进行合并，然后再进行微调即可。

步骤1：关闭速度环，只调直立环

   这里先确保个个环都是能够正常使用的，先调试直立环，当使用直立环时，我们会发现小车基本能够平衡一段时间。
   正确效果：当小车往哪边倾斜时，小车就往哪边加速，如果加速方向相反，则将pwm值取反即可。

代码如下：

PID Balance_PID;  //直立环

int EXTI9_5_IRQHandler(void) 
{    
	if(INT==0)		   //当陀螺仪的数据变化时，该引脚为低
	{   
		EXTI_ClearITPendingBit(EXTI_Line9);
		Get_Angle();
		if(Turn_Off(Angle_Balance))
		{
			Balance_Pwm = PID_Balance_Calc(&Balance_PID, Angle_Balance, Gyro_Balance);
			
			Set_Pwm(Left_moter,-Balance_Pwm );  //3.PWM输出给电机
			Set_Pwm(Right_moter,-Balance_Pwm );  //3.PWM输出给电机 
		}
	}
	 return 0;	 
}

//-------------主函数
int main()
{
   PID_Init(&Balance_PID, 80000, 0, 10, 3000);  //初始化pid
   while(1){}
}

步骤2：关闭直立环，只调速度环

  正确效果：当我们转动其中一个轮子时，另一个轮子也朝着我们转动的方向一起转动，则为正反馈。如果其中轮子运动方向相反，则将其中一个编码器数值取反即可。

代码如下：

PID Speed_PID;  //速度环

int EXTI9_5_IRQHandler(void) 
{    
	if(INT==0)		   //当陀螺仪的数据变化时，该引脚为低
	{   
		EXTI_ClearITPendingBit(EXTI_Line9);
		Get_Angle();
		if(Turn_Off(Angle_Balance))
		{
			Encoder_Left  = -TIM4_ReadEncoder();    
			Encoder_Right = TIM1_ReadEncoder(); 
			
			Velocity_Pwm=PID_Speed_Calc(&Speed_PID,Encoder_Left,Encoder_Right);
			Set_Pwm(Left_moter, Velocity_Pwm);  //3.PWM输出给电机
			Set_Pwm(Right_moter, Velocity_Pwm);  //3.PWM输出给电机 
		}
	}
	 return 0;	 
}

//-------------主函数
int main()
{
   PID_Init(&Speed_PID, 3, 0.8, 0, 10000);  //初始化pid
   while(1){}
}

步骤3：双环合并

PID Balance_PID;  //直立环
PID Speed_PID;  //速度环

int EXTI9_5_IRQHandler(void) 
{    
	int Motor_Left,Motor_Right;      //电机PWM变量 应是Motor的 向Moto致敬	
	if(INT==0)		   //当陀螺仪的数据变化时，该引脚为低
	{   
		EXTI_ClearITPendingBit(EXTI_Line9);
		Get_Angle();
		if(Turn_Off(Angle_Balance))
		{
				Encoder_Left  = -TIM4_ReadEncoder();    
				Encoder_Right = TIM1_ReadEncoder(); 
				
				Balance_Pwm = PID_Balance_Calc(&Balance_PID, Angle_Balance, Gyro_Balance);
				Velocity_Pwm=PID_Speed_Calc(&Speed_PID, Encoder_Left, Encoder_Right);
				
				Motor_Left= Velocity_Pwm+Balance_Pwm;
				Motor_Right=Velocity_Pwm+Balance_Pwm;
			
				Motor_Left =PWM_Limit(Motor_Left,3000,-3000); //限幅
				Motor_Right=PWM_Limit(Motor_Right,3000,-3000);
				
				Set_Pwm(Left_moter,  Motor_Left );  //3.PWM输出给电机
				Set_Pwm(Right_moter, Motor_Right);  //3.PWM输出给电机 
		}
	}
	 return 0;	 
}

//-------------主函数
int main()
{
   PID_Init(&Speed_PID, 3, 0.8, 0, 10000);  //初始化pid
   PID_Init(&Balance_PID, 80000, 0, 10, 3000);  //初始化pid
   while(1){}
}

三、效果演示

平衡小车