stm32——pid双闭环、编码器控制小车转向预设角度

digital_icer · 2022 年2 月 5 日 09:32

一、项目来源
之前参加电子一个比赛，做类似智障小车的东西，程序基本上写的没问题，但是由于比赛环境影响和场地因素没有很好完成比赛。在这里记录一下关于这辆智障小车比较核心的东西。
二、 .硬件清单和说明
1.主控：STM32F4探索者
2.电机：带编码器的减速电机
3.电机驱动：tb6612
4.光敏传感器、湿度传感器、红外传感器、烟雾传感器等等
三、机械零件
1、小车底盘
2、普通车轮
3、电机座
4、铜柱、螺丝若干
*四、控制原理
通过调节pwm占空比来调节电机转动的角度。编码器检测电机转动过程中产生的脉冲，计算出电机转过的角度，通过调节pid参数使得转动角度达到较为理想的状态。这里嫖一点隔壁的东西。

如何测量车轮转速
可以通过安装在电机输出轴上的活儿编码器来测量得到小车的车轮速度。利用控制单片机的计数器测量在固定时间间隔内速度脉冲信号的个数可以反映电机的转速。

什么是pid算法

这里推荐b站一个阿婆主，是robomaster华南理工大学华南虎战队，华南虎战队是rm的传统强队，其对pid的讲解也是细致入微。3懂的都懂（PID通俗公式理解）_哔哩哔哩_bilibili

五、代码实现
讲了这么多废话也没有讲明白，不如来看看到底怎么写的

pwm输出给到电机是首要的
这里分别使用了两个定时器产生两路pwm波分别给到两个电机

#include "pwm.h"
#include "usart.h"
 
/*****************************************************
函数功能：pwm初始化
入口参数：arr自动重装值 psc时钟预分频数
返回值：无
*****************************************************/
void TIM2_PWM_Init(uint16_t arr,uint16_t psc)
{     
    GPIO_InitTypeDef  GPIO_InitStructure;        
    TIM_TimeBaseInitTypeDef  TIM_TimeBaseStructure;
    TIM_OCInitTypeDef  TIM_OCInitStructure;
     
    RCC_APB1PeriphClockCmd(RCC_APB1Periph_TIM2,ENABLE);    	//TIM2时钟使能  
    RCC_AHB1PeriphClockCmd(RCC_AHB1Periph_GPIOA,ENABLE);   	//使能PORTA时钟
        
    GPIO_PinAFConfig(GPIOA,GPIO_PinSource0,GPIO_AF_TIM2);  	//GPIOA0复用为定时器2
	 
     
	  GPIO_InitStructure.GPIO_Pin = GPIO_Pin_0;          //GPIOA0
    GPIO_InitStructure.GPIO_Mode = GPIO_Mode_AF;       //复用功能
    GPIO_InitStructure.GPIO_OType = GPIO_OType_PP;     //推挽复用输出
    GPIO_InitStructure.GPIO_PuPd = GPIO_PuPd_NOPULL;   //不上下拉
    GPIO_InitStructure.GPIO_Speed = GPIO_Speed_100MHz; //速度100MHz
    GPIO_Init(GPIOA,&GPIO_InitStructure);              //初始化PA0
     
	  TIM_TimeBaseStructure.TIM_Prescaler = psc;            			 //定时器分频
   	TIM_TimeBaseStructure.TIM_CounterMode = TIM_CounterMode_Up;   	 //向上计数模式
	  TIM_TimeBaseStructure.TIM_Period = arr;                       	 //自动重装载值
	  TIM_TimeBaseStructure.TIM_ClockDivision = TIM_CKD_DIV1;        
    TIM_TimeBaseInit(TIM2,&TIM_TimeBaseStructure);          		 //初始化定时器1
                
    //初始化TIM1 Channel1,2
    TIM_OCInitStructure.TIM_OCMode = TIM_OCMode_PWM1;     			//选择定时器模式:TIM脉冲宽度调制模式2
    TIM_OCInitStructure.TIM_OutputState = TIM_OutputState_Enable;  	//比较输出使能
    TIM_OCInitStructure.TIM_Pulse = arr;
    TIM_OCInitStructure.TIM_OCPolarity = TIM_OCPolarity_High;  		//输出极性高

		TIM_OC1Init(TIM2, &TIM_OCInitStructure);    //根据T指定的参数初始化外设OC1
		
		
		TIM_OC1PreloadConfig(TIM2, TIM_OCPreload_Enable);   //使能TIM1在CCR1上的预装载寄存器
		
		
		TIM_ARRPreloadConfig(TIM2,ENABLE);			//ARPE使能 
		TIM_CtrlPWMOutputs(TIM2,ENABLE); 			//使能TIM1的PWM输出
		TIM_Cmd(TIM2,ENABLE);						//使能TIM1                         
}

/*****************************************************
函数功能：pwm初始化
入口参数：arr自动重装值 psc时钟预分频数
返回值：无
*****************************************************/
void TIM14_PWM_Init(uint32_t arr,uint32_t psc)
{		 					 
	//此部分需手动修改IO口设置
	
	GPIO_InitTypeDef GPIO_InitStructure;
	TIM_TimeBaseInitTypeDef  TIM_TimeBaseStructure;
	TIM_OCInitTypeDef  TIM_OCInitStructure;
	
	RCC_APB1PeriphClockCmd(RCC_APB1Periph_TIM14,ENABLE);  	//TIM14时钟使能    
	RCC_AHB1PeriphClockCmd(RCC_AHB1Periph_GPIOF, ENABLE); 	//使能PORTF时钟	
	
	GPIO_PinAFConfig(GPIOF,GPIO_PinSource9,GPIO_AF_TIM14); //GPIOF9复用为定时器14
	
	GPIO_InitStructure.GPIO_Pin = GPIO_Pin_9;           //GPIOF9
	GPIO_InitStructure.GPIO_Mode = GPIO_Mode_AF;        //复用功能
	GPIO_InitStructure.GPIO_Speed = GPIO_Speed_100MHz;	//速度100MHz
	GPIO_InitStructure.GPIO_OType = GPIO_OType_PP;      //推挽复用输出
	GPIO_InitStructure.GPIO_PuPd = GPIO_PuPd_NOPULL;        //上拉
	GPIO_Init(GPIOF,&GPIO_InitStructure);              //初始化PF9
	  
	TIM_TimeBaseStructure.TIM_Prescaler=psc;  //定时器分频
	TIM_TimeBaseStructure.TIM_CounterMode=TIM_CounterMode_Up; //向上计数模式
	TIM_TimeBaseStructure.TIM_Period=arr;   //自动重装载值
	TIM_TimeBaseStructure.TIM_ClockDivision=TIM_CKD_DIV1; 
	
	TIM_TimeBaseInit(TIM14,&TIM_TimeBaseStructure);//初始化定时器14
	
	//初始化TIM14 Channel1 PWM模式	 
	TIM_OCInitStructure.TIM_OCMode = TIM_OCMode_PWM1; //选择定时器模式:TIM脉冲宽度调制模式2
 	TIM_OCInitStructure.TIM_OutputState = TIM_OutputState_Enable; //比较输出使能
    TIM_OCInitStructure.TIM_Pulse = arr;
    TIM_OCInitStructure.TIM_OCPolarity = TIM_OCPolarity_High;  		//输出极性高
		TIM_OC1Init(TIM14, &TIM_OCInitStructure);  //根据T指定的参数初始化外设TIM1 4OC1

	TIM_OC1PreloadConfig(TIM14, TIM_OCPreload_Enable);  //使能TIM14在CCR1上的预装载寄存器
 
  TIM_ARRPreloadConfig(TIM14,ENABLE);//ARPE使能 
	
	TIM_Cmd(TIM14, ENABLE);  //使能TIM14
 
										  
}

位置式大家可以自行根据pid公式写

float PID_GetPositionPID(PID_t* WhichPID)
{
	WhichPID->PID_Out = WhichPID->Kp1 * WhichPID->PID_Err_now + WhichPID->Kd1 * (WhichPID->PID_Err_now - WhichPID->PID_Err_last);
	WhichPID->PID_Out += ( WhichPID->PID_Err_all * WhichPID->Ki1 );
	
	if(WhichPID->PID_Out >= WhichPID->PID_OutMax)
		WhichPID->PID_Out = WhichPID->PID_OutMax;
	if(WhichPID->PID_Out <= -WhichPID->PID_OutMax)
		WhichPID->PID_Out = -WhichPID->PID_OutMax;
	
	if(WhichPID->PID_Out - WhichPID->PID_lastout > WhichPID->PID_OutStep)
	{
		WhichPID->PID_Out = WhichPID->PID_lastout + WhichPID->PID_OutStep;
	}
	if(WhichPID->PID_Out - WhichPID->PID_lastout < -WhichPID->PID_OutStep)
	{
		WhichPID->PID_Out = WhichPID->PID_lastout + -WhichPID->PID_OutStep;
	}
	
	WhichPID->PID_lastout = WhichPID->PID_Out;

	return WhichPID->PID_Out;
}

增量式

float PID_GetIncrementalPID(PID_t* WhichPID)
{
	WhichPID->PID_Out += WhichPID->Ki1 * WhichPID->PID_Err_now + WhichPID->Kp1 * (WhichPID->PID_Err_now - WhichPID->PID_Err_last);
	WhichPID->PID_Out += WhichPID->Kd1 * (WhichPID->PID_Err_now - 2 * WhichPID->PID_Err_last + WhichPID->PID_Err_lastlast);

	if(WhichPID->PID_Out >= WhichPID->PID_OutMax)
		WhichPID->PID_Out = WhichPID->PID_OutMax;
	if(WhichPID->PID_Out <= -WhichPID->PID_OutMax)
		WhichPID->PID_Out = -WhichPID->PID_OutMax;

	if(WhichPID->PID_Out - WhichPID->PID_lastout > WhichPID->PID_OutStep)
	{
		WhichPID->PID_Out = WhichPID->PID_lastout + WhichPID->PID_OutStep;
	}
	if(WhichPID->PID_Out - WhichPID->PID_lastout < -WhichPID->PID_OutStep)
	{
		WhichPID->PID_Out = WhichPID->PID_lastout + -WhichPID->PID_OutStep;
	}
	
	WhichPID->PID_lastout = WhichPID->PID_Out;	
	return WhichPID->PID_Out;
}

在位置式和增量式中都已经进行了限幅，不用再单独编写限幅函数，pid参数初始化和清零函数不再赘述

这里需要一个函数更新pid的数据

void PID_Update(PID_t* WhichPID, float NowInput)
{//更新PID的数据，即更新PID的输入值
	WhichPID->PID_Input = NowInput;
	if(WhichPID->State_RampOrNormal == Ramp_e)
	{
		if(WhichPID->RampCountTime < WhichPID->RampTartgetTime)
		{
			++ WhichPID ->RampCountTime;
		}
		else
		{
			WhichPID->RampCountTime = 0;
			if(WhichPID->PID_Target <  WhichPID->RampTarget )
			{//斜坡函数还没计数完，
				WhichPID->PID_Target += WhichPID->RampTartgetStep;
				if(WhichPID->PID_Target >= WhichPID->RampTarget)
				{
					WhichPID->PID_Target = WhichPID->RampTarget;
					WhichPID->State_RampOrNormal = Normal_e;
				}
			}
			else if(WhichPID->PID_Target >  WhichPID->RampTarget)
			{
				WhichPID->PID_Target -= WhichPID->RampTartgetStep;
				if(WhichPID->PID_Target <= WhichPID->RampTarget)
				{
					WhichPID->PID_Target = WhichPID->RampTarget;
					WhichPID->State_RampOrNormal = Normal_e;
				}
			}
			else
			{//斜坡函数计数已经完成，要退出斜坡函数模式
				WhichPID->State_RampOrNormal = Normal_e;
			}
		}
	}

在此之后，需要一个函数将位置式和增量式的反馈赋值给速度环和位置环，而后需要一个定时器中断

void TIM5_IRQHandler(void)
{
	if(TIM_GetITStatus(TIM5,TIM_IT_Update) == SET)
	{//溢出中断				
		++ Timer5Flag;

		if(Timer5Flag % 10 == 0)
		{
			updata_Encoder();
			Speedloop();
			ERR=AngleB;//
			err=Encoder_SpeedB;
		}
		
		if(Timer5Flag % 50 == 0)
		{
			if(state==Left||state==Right||state==Back)
			Positionloop();
		}
		
		if(Timer5Flag == 100)
		{
			Timer5Flag = 0;     
			//LED_Run();       			
		}   
	}	 
	TIM_ClearITPendingBit(TIM5,TIM_IT_Update);  //清除中断标志位
}

下面motor.c中小车左转为例

void  ZZXC_Left()//左转函数
{
	Last_state=state;
	state=Left;
	if(state!=Last_state)
	{
		Clear();
		PositionPID_A.PID_Target=1740;
		PositionPID_A.State_RampOrNormal=Normal_e;
		PositionPID_B.PID_Target=120;
		PositionPID_B.State_RampOrNormal=Normal_e;
	}
  GPIO_SetBits(Left_Motor_Go_GPIO,Left_Motor_Go_Pin1);
	GPIO_ResetBits(Left_Motor_Go_GPIO,Left_Motor_Go_Pin2);
	GPIO_SetBits(Right_Motor_Go_GPIO,Right_Motor_Go_Pin1);
	GPIO_ResetBits(Right_Motor_Go_GPIO,Right_Motor_Go_Pin2);
}
这里转弯根据自己搭建的赛道设置成了漂移转弯的形式，可以自行进行参数设定改为原地转向

通过pid和编码器转向只能作为一种辅助手段，需要i依靠其它参照如视觉或者巡线李进行校正。
文章写得属实有点拙劣，难为我这个语文拉跨选手了。

maker233666 · 2022 年2 月 21 日 11:09

cy，等一手更新