[嵌入式]PID算法实现直流减速电机控速

由于资源有限，人也比较懒，目前只实现了一个电机的控速，想着需要的时候，薅过去就能直接用，因为设想中是要控制俩编码器电机的，所以代码中会看到有些引脚配置了，但却没有在接线图中看到，直接忽略只看用到了的就好。

目录

大致框架

硬件资源

硬件连线

stm32资源使用情况

PID控速

代码实现

大致框架

????????????????????????

硬件资源

STM32f103c8t6最小系统板；直流减速电机；电机驱动模块L298N；12V电源；

硬件连线

元件引脚示意（实物图都网上找的）：

连线图：

stm32资源使用情况

TIM1：用于定时，同时配置了中断，每20ms进入一次中断函数

TIM2：利用c1和c2两个引脚捕获电机A、B相脉冲，结合定时器1可实现PID控速

TIM4：c1~c4随便选一个引脚输出PWM直接控制电机

PID控速

PID我也是第一次用，用之前在网上找了很多参考文章，在B站也看了有关视频，得出结论：PID有位置式PID和增量式PID，因为之后要实现的小车并不能获取当前位置和目的地的距离，所以这里用增量式PID，由当前速度经PID算法获得合适的PWM。期间主要参考了这两篇文章：PID理解?、代码实现?

下面这段代码在代码里面也有，这里单独截出来：

?? ??? ??? ?s.curr_val=cnt2*50/4-cnt1*50/4;//当前电机转速（周期/s）
?? ??? ??? ?s.err=s.expect_val-s.curr_val;//本次误差
?? ??? ??? ?p_out=s.P*(s.err-s.err_pre);
?? ??? ??? ?i_out=s.I*s.err;
?? ??? ??? ?d_out=s.D*(s.err-2*s.err_pre+s.err_pre_pre);//参量D这里其实没有用到，一直是0
?? ??? ??? ?pwm+=p_out+i_out+d_out;
?? ??? ??? ?s.err_pre_pre=s.err_pre;//保存上上次误差
?? ??? ??? ?s.err_pre=s.err;//保存上次误差

当前速度（单位：周期/s）计算思路：

图1：

????????????????????????????????

图2：

?

电机转动时，结合编码器会在A、B相分别输出脉冲（如上图1），如果时间间隔足够短并且周期不发生突变的话，我们可以认为A、B两相都是周期固定的波形，TIM2的c1和c2两脚获取A、B相的波形，在不用知道电机转动方向的前提下可以只使用一相，假设c1脚每捕获到一个上升沿，CNT就加一（图2可以看到编码器接口的时钟来自TIMx_CH1和TIMx_CH2），那么在固定的时间间隔内（由TIM1计时提供），可以得出该段时间内电机编码器输出了多少个上升沿（周期），即电机转动速度。具体计算在代码里面有。

代码实现

#include "stm32f10x.h"                  // Device header
#include "Delay.h"
#include "usart.h"

void rcc_config(void);
void direction_GPIO_init(void);
void timer_catch_init(void);//TIM2/3
void timer_PWM_init(void);//TIM4
void timer_init_2(void);//TIM1:计时
void exti_init(void);//EXTI2/3（还没有实际用过，忽略就好）
void NVIC_config(void);
void forward(u16 speed);
void right(u16 speed);


typedef struct
{
	float P;
	float I;
	float D;
	float expect_val;//期望值（每秒多少个脉冲）
	float curr_val;//当前值（每秒多少个脉冲）
	float err_pre;//上一次的误差
	float err_pre_pre;//上上次的误差
	float err;//当前值和期望值误差
}PID;

PID s;
float pwm=0;

//PID结构体初始化，P、I、D三个值都要根据实际情况慢慢调
void PID_Init(void)
{
	s.P=1;
	s.I=0.2;//0.2
	s.D=0;
	s.err_pre=0;
	s.err_pre_pre=0;
	s.err=0;
	s.curr_val=0;
	s.expect_val=800;
}

int main() 
{
	PID_Init();
	
//	USART_config();
	
	rcc_config();
	direction_GPIO_init();
	timer_catch_init();
	timer_PWM_init();
	timer_init_2();
//	exti_init();
	NVIC_config();
	while(1)
	{
		if(pwm>=1000)
			pwm=999;
		else if(pwm<=0)
			pwm=0;
		forward((u16)pwm);
	}
}

void rcc_config(void)
{
	RCC_APB1PeriphClockCmd(RCC_APB1Periph_TIM2|RCC_APB1Periph_TIM3,ENABLE);
	RCC_APB2PeriphClockCmd(RCC_APB2Periph_GPIOA|RCC_APB2Periph_GPIOB,ENABLE);
	RCC_APB1PeriphClockCmd(RCC_APB1Periph_TIM4,ENABLE);
	RCC_APB2PeriphClockCmd(RCC_APB2Periph_AFIO|RCC_APB2Periph_TIM1,ENABLE);
}

void direction_GPIO_init(void)
{
	GPIO_InitTypeDef gpio_structure;
	gpio_structure.GPIO_Mode=GPIO_Mode_Out_PP;
	gpio_structure.GPIO_Pin=GPIO_Pin_11|GPIO_Pin_12|GPIO_Pin_10|GPIO_Pin_15;
	gpio_structure.GPIO_Speed=GPIO_Speed_50MHz;
	GPIO_Init(GPIOA,&gpio_structure);
	GPIO_ResetBits(GPIOA,GPIO_Pin_11|GPIO_Pin_12|GPIO_Pin_10|GPIO_Pin_15);
}

void timer_catch_init(void)
{	
	/*---GPIO口配置---*/
	//TIM2_CH1--PA0 TIM2_CH2--PA1 TIM3_CH1--PA6 TIM3_CH2--PA7
	GPIO_InitTypeDef gpio_structure;
	gpio_structure.GPIO_Mode=GPIO_Mode_IN_FLOATING;//浮空输入
	gpio_structure.GPIO_Pin=GPIO_Pin_0|GPIO_Pin_1|GPIO_Pin_6|GPIO_Pin_7;
	gpio_structure.GPIO_Speed=GPIO_Speed_50MHz;
	GPIO_Init(GPIOA,&gpio_structure);
	
	/*---TIM配置---*/
	//-----时基单元初始化------
	TIM_TimeBaseInitTypeDef TIM_TimeBaseStruct;
	TIM_TimeBaseStruct.TIM_Prescaler=1-1;//预分频器
 	TIM_TimeBaseStruct.TIM_Period=65536-1;//自动重装载寄存器
	TIM_TimeBaseStruct.TIM_ClockDivision=0;
	TIM_TimeBaseStruct.TIM_CounterMode=TIM_CounterMode_Up;
	TIM_TimeBaseInit(TIM2,&TIM_TimeBaseStruct);
	TIM_TimeBaseInit(TIM3,&TIM_TimeBaseStruct);	
		
	//编码器模式(同时也选择了时钟来源)
	TIM_EncoderInterfaceConfig(TIM2,TIM_EncoderMode_TI12,
							TIM_ICPolarity_BothEdge,
							TIM_ICPolarity_BothEdge);
	TIM_EncoderInterfaceConfig(TIM3,TIM_EncoderMode_TI12,
							TIM_ICPolarity_BothEdge,
							TIM_ICPolarity_BothEdge);
							
	//-----输入捕获-----
	TIM_ICInitTypeDef TIM_ICInitStruct;
	TIM_ICInitStruct.TIM_Channel = TIM_Channel_1|TIM_Channel_2;
	TIM_ICInitStruct.TIM_ICPolarity = TIM_ICPolarity_BothEdge;
	TIM_ICInitStruct.TIM_ICSelection = TIM_ICSelection_DirectTI;
	TIM_ICInitStruct.TIM_ICPrescaler = TIM_ICPSC_DIV1;
	TIM_ICInitStruct.TIM_ICFilter = 0x00;
	//TIM_ICStructInit(&TIM_ICInitStruct);
	TIM_ICInit(TIM2,&TIM_ICInitStruct);
	TIM_ICInit(TIM3,&TIM_ICInitStruct);
	
	TIM_Cmd(TIM2,ENABLE);//使能定时器2
	TIM_Cmd(TIM3,ENABLE);//使能定时器3
}

void timer_PWM_init(void)
{	
	GPIO_InitTypeDef gpio_structure;
	/*---GPIO配置---*/
	gpio_structure.GPIO_Mode=GPIO_Mode_AF_PP;//推挽输出
	gpio_structure.GPIO_Pin=GPIO_Pin_6|GPIO_Pin_7|GPIO_Pin_8|GPIO_Pin_9;
	gpio_structure.GPIO_Speed=GPIO_Speed_50MHz;
	GPIO_Init(GPIOB,&gpio_structure);
	//初始为低电平
	GPIO_SetBits(GPIOB,GPIO_Pin_6|GPIO_Pin_7|GPIO_Pin_8|GPIO_Pin_9);
	
	TIM_InternalClockConfig(TIM4);//选择内部时钟（1、时钟选择）
	
	//-----时基单元初始化------
	TIM_TimeBaseInitTypeDef TIM_TimeBaseStruct;
	
	TIM_TimeBaseStruct.TIM_Prescaler=360-1;//预分频器 5ms
 	TIM_TimeBaseStruct.TIM_Period=1000-1;
	TIM_TimeBaseStruct.TIM_ClockDivision=0;
	TIM_TimeBaseStruct.TIM_CounterMode=TIM_CounterMode_Up;
	TIM_TimeBaseInit(TIM4,&TIM_TimeBaseStruct);
	
	//-----输出比较-----
	TIM_OCInitTypeDef TIM_OCStruct;

	TIM_OCStruct.TIM_OCMode=TIM_OCMode_PWM1;
	TIM_OCStruct.TIM_OCPolarity=TIM_OCPolarity_High;
	TIM_OCStruct.TIM_OutputState=TIM_OutputState_Enable;
	TIM_OCStruct.TIM_Pulse=0;//TIM_Pulse/(TIM_Period+1)=占空比
	//初始化外设TIM2通道
	TIM_OC1Init(TIM4,&TIM_OCStruct);
	TIM_OC2Init(TIM4,&TIM_OCStruct);
	TIM_OC3Init(TIM4,&TIM_OCStruct);
	TIM_OC4Init(TIM4,&TIM_OCStruct);
	
	TIM_Cmd(TIM4,ENABLE);//使能定时器4
}

void timer_init_2(void)
{
	TIM_InternalClockConfig(TIM1);//选择内部时钟（1、时钟选择）
	
	//-----时基单元初始化------
	TIM_TimeBaseInitTypeDef TIM_TimeBaseStruct;
	
	TIM_TimeBaseStruct.TIM_Prescaler=1000-1;//预分频器
 	TIM_TimeBaseStruct.TIM_Period=1440-1;//100ms
	TIM_TimeBaseStruct.TIM_ClockDivision=TIM_CKD_DIV1;
	TIM_TimeBaseStruct.TIM_CounterMode=TIM_CounterMode_Up;
	TIM_TimeBaseStruct.TIM_RepetitionCounter=0;
	TIM_TimeBaseInit(TIM1,&TIM_TimeBaseStruct);
	TIM_ClearFlag(TIM1,TIM_FLAG_Update);
	TIM_ITConfig(TIM1,TIM_IT_Update,ENABLE);
	TIM_Cmd(TIM1,ENABLE);
}

void exti_init()
{
	//2、配置GPIO
	//--PA2、PA3--
	GPIO_InitTypeDef gpio_struct;
	gpio_struct.GPIO_Mode=GPIO_Mode_IN_FLOATING;//查看手册可得
	gpio_struct.GPIO_Pin=GPIO_Pin_3|GPIO_Pin_2;
	gpio_struct.GPIO_Speed=GPIO_Speed_50MHz;
	GPIO_Init(GPIOA,&gpio_struct);
	//3、配置AFIO
	GPIO_EXTILineConfig(GPIO_PortSourceGPIOA,GPIO_PinSource2);
	GPIO_EXTILineConfig(GPIO_PortSourceGPIOA,GPIO_PinSource3);
	//4、配置EXTI
	EXTI_InitTypeDef exti_struct;
	exti_struct.EXTI_Line=EXTI_Line2;
	exti_struct.EXTI_LineCmd=ENABLE;
	exti_struct.EXTI_Mode=EXTI_Mode_Interrupt;//中断模式
	exti_struct.EXTI_Trigger=EXTI_Trigger_Rising;//上升沿触发
	EXTI_Init(&exti_struct);
	exti_struct.EXTI_Line=EXTI_Line3;
	EXTI_Init(&exti_struct);
}

void NVIC_config(void)
{
	NVIC_PriorityGroupConfig(NVIC_PriorityGroup_2);//分组方式
	NVIC_InitTypeDef nvic_struct;
	nvic_struct.NVIC_IRQChannel=EXTI2_IRQn;
	nvic_struct.NVIC_IRQChannelCmd=ENABLE;
	nvic_struct.NVIC_IRQChannelPreemptionPriority=1;
	nvic_struct.NVIC_IRQChannelSubPriority=1;
	NVIC_Init(&nvic_struct);
	
	nvic_struct.NVIC_IRQChannel=EXTI3_IRQn;
	nvic_struct.NVIC_IRQChannelCmd=ENABLE;
	nvic_struct.NVIC_IRQChannelPreemptionPriority=1;
	nvic_struct.NVIC_IRQChannelSubPriority=1;
	NVIC_Init(&nvic_struct);

	nvic_struct.NVIC_IRQChannel=TIM1_UP_IRQn;
	nvic_struct.NVIC_IRQChannelCmd=ENABLE;
	nvic_struct.NVIC_IRQChannelPreemptionPriority=1;
	nvic_struct.NVIC_IRQChannelSubPriority=1;
	NVIC_Init(&nvic_struct);
}

void forward(u16 speed)
{
	/*****/
	TIM_SetCompare1(TIM4,speed);
	GPIO_SetBits(GPIOA,GPIO_Pin_10);
	GPIO_ResetBits(GPIOA,GPIO_Pin_11);
}

void right(u16 speed)
{
	/*****/
}

/*************中断函数***********************/
//每20ms进一次中断
float cnt1=-1,cnt2=-1;
u8 flag=1;
float p_out=0,i_out=0,d_out=0;
u8 times=0;
void TIM1_UP_IRQHandler(void)
{
	if(TIM_GetITStatus(TIM1,TIM_IT_Update)==SET)
	{
		if(flag==1)
		{
			flag=2;
			cnt1=TIM_GetCounter(TIM2);
		}
		else
		{
			flag=1;
			cnt2=TIM_GetCounter(TIM2);
		}
		if(flag==1 && cnt1!=-1 && cnt2!=-1)
		{
			if(cnt2<cnt1)
				cnt2=65535+cnt2;
			s.curr_val=cnt2*50/4-cnt1*50/4;//当前电机转速（周期/s）
			s.err=s.expect_val-s.curr_val;//本次误差
			p_out=s.P*(s.err-s.err_pre);
			i_out=s.I*s.err;
			d_out=s.D*(s.err-2*s.err_pre+s.err_pre_pre);//参量D这里其实没有用到，一直是0
			pwm+=p_out+i_out+d_out;
			s.err_pre_pre=s.err_pre;//保存上上次误差
			s.err_pre=s.err;//保存上次误差
		}
		TIM_ClearITPendingBit(TIM1,TIM_IT_Update);
	}
}

期间踩过的坑：

1、在测试阶段，发现L298N不能成功驱动电机（在电路连接正确和电源条件满足的情况），后来发现，L298N的5V和GND引脚需要引出来给别的器件供电才能正常驱动电机（到底为啥也不知道）

2、PID调参：看了很多相关文章都说PID三个参数的调试顺序为P、I、D，后来发现这应该只是针对位置式PID，仔细看看，会发现增量式PID的I项和位置式PID的P项要乘的东西是一样的，所以在增量式PID中，可以先尝试调I项。