【电机控制】小白从零开始：STM32双闭环（速度环、位置环）电机控制（软件篇）

Kisorge

已于 2023-11-26 13:57:31 修改

阅读量2.5k

点赞数 3

分类专栏：电机控制文章标签： stm32 单片机嵌入式硬件

于 2023-04-10 12:15:53 首次发布

本文链接：https://blog.csdn.net/qq_42681425/article/details/130056824

版权

电机控制专栏收录该内容

6 篇文章 12 订阅

订阅专栏

小白从零开始：STM32双闭环（速度环、位置环）电机控制（软件篇）

文章目录

前言
一、电机测速
二、电机PID控制算法
三、电机PWM输出
四、双闭环速度环在内、位置环在外
总结

小白从零开始：STM32双闭环（速度环、位置环）电机控制（硬件篇）

前言

小白从零开始：STM32双闭环（速度环、位置环）电机控制（软件篇）

杭州研究生手把手教你搞不定STM32

使用工具：
1.语言：C语言
2.代码编译：KEIL5、
3.代码烧录：FLYMCU

请添加图片描述

提示：以下是本篇文章正文内容，下面案例可供参考

一、电机测速

#include "encoder.h"
void Encoder_TIM4_Init(void)
{      
	NVIC_InitTypeDef NVIC_InitStructure;
	GPIO_InitTypeDef GPIO_InitStructure;
	TIM_TimeBaseInitTypeDef  TIM_TimeBaseStructure;
	TIM_ICInitTypeDef TIM_ICInitStructure;
//      NVIC_InitTypeDef    NVIC_InitStructure;	

	RCC_APB1PeriphClockCmd(RCC_APB1Periph_TIM4, ENABLE);//Ê¹ÄÜ¶¨Ê±Æ÷4µÄÊ±ÖÓ
	RCC_APB2PeriphClockCmd(RCC_APB2Periph_GPIOB, ENABLE);//Ê¹ÄÜ¶Ë¿ÚB
	
	/*- Õý½»±àÂëÆ÷ÊäÈëÒý½Å PB->6   PB->7 -*/
	GPIO_InitStructure.GPIO_Pin = GPIO_Pin_6 | GPIO_Pin_7;//¶Ë¿ÚÅäÖÃ      
	GPIO_InitStructure.GPIO_Mode = GPIO_Mode_IN_FLOATING;//¸¡¿ÕÊäÈë
	GPIO_InitStructure.GPIO_Speed = GPIO_Speed_50MHz;
	GPIO_Init(GPIOB, &GPIO_InitStructure); //³õÊ¼»¯GPIOB                        

	/*- TIM4±àÂëÆ÷Ä£Ê½ÅäÖÃ -*/
	//TIM_DeInit(TIM4); 
	TIM_TimeBaseStructure.TIM_Period = 65535;//¼Æ´ÎÊý;ÉèÖÃ¼ÆÊýÆ÷×Ô¶¯ÖØ×°ÔØ
	TIM_TimeBaseStructure.TIM_Prescaler = 0;//Ô¤·ÖÆµÆ÷
	TIM_TimeBaseStructure.TIM_ClockDivision =TIM_CKD_DIV1 ;//Ñ¡ÔñÊ±ÖÓ·ÖÆµÄ£Ê½£º²»·ÖÆµ
	TIM_TimeBaseStructure.TIM_CounterMode = TIM_CounterMode_Up;//Á¬ÐøÏòÉÏ¼ÆÊýÄ£Ê½
	TIM_TimeBaseInit(TIM4, &TIM_TimeBaseStructure);              
                 
	TIM_EncoderInterfaceConfig(TIM4, TIM_EncoderMode_TI12, TIM_ICPolarity_BothEdge ,TIM_ICPolarity_BothEdge);	//ÅäÖÃ±àÂëÆ÷Ä£Ê½´¥·¢Ô´ºÍ¼«ÐÔË«±ßÑØ¼ì²â ²»ÖªµÀ¿ÉÒÔ¿´¼¼ÊõÊÖ²áÉÏÃæÓÐ½éÉÜ±àÂëÆ÷Ä£Ê½
	//Ê¹ÓÃ±àÂëÆ÷Ä£Ê½3
	TIM_ICInitStructure.TIM_Channel = TIM_Channel_1; //CC1S=01 	Ñ¡ÔñÊäÈë¶Ë IC1Ó³Éäµ½TI1ÉÏ
 	TIM_ICInitStructure.TIM_ICPolarity = TIM_ICPolarity_BothEdge;	//ÉÏÉýÑØ²¶»ñ
 	TIM_ICInitStructure.TIM_ICSelection = TIM_ICSelection_DirectTI; //Ó³Éäµ½TI1ÉÏ
 	TIM_ICInitStructure.TIM_ICPrescaler = TIM_ICPSC_DIV1;	 //ÅäÖÃÊäÈë·ÖÆµ,²»·ÖÆµ
 	TIM_ICInitStructure.TIM_ICFilter = 0x01;//IC1F=0000 ÅäÖÃÊäÈëÂË²¨Æ÷ ²»ÂË
// 
	TIM_ICInit(TIM4, &TIM_ICInitStructure);
	NVIC_InitStructure.NVIC_IRQChannel = TIM4_IRQn;
	NVIC_InitStructure.NVIC_IRQChannelPreemptionPriority=2 ;//ÇÀÕ¼ÓÅÏÈ¼¶2
	NVIC_InitStructure.NVIC_IRQChannelSubPriority = 3;		//×ÓÓÅÏÈ¼¶3
	NVIC_InitStructure.NVIC_IRQChannelCmd = ENABLE;			//IRQÍ¨µÀÊ¹ÄÜ
	NVIC_Init(&NVIC_InitStructure);	//¸ù¾ÝÖ¸¶¨µÄ²ÎÊý³õÊ¼»¯VIC¼Ä´æÆ÷
	
	TIM_ClearFlag(TIM4, TIM_FLAG_Update);
      TIM_ITConfig(TIM4, TIM_IT_Update, ENABLE); 	
	TIM_SetCounter(TIM4,0);
	TIM_Cmd(TIM4, ENABLE);   //Æô¶¯TIM4¶¨Ê±Æ÷
 }

__IO int16_t EncoderOverflowCnt = 0;
//¶¨Ê±Æ÷4ÖÐ¶Ï·þÎñ³ÌÐò
void TIM4_IRQHandler(void)
{
	if (TIM_GetITStatus(TIM4, TIM_IT_Update) != RESET)
	{
		if((TIM4->CR1 & TIM_CounterMode_Down) != TIM_CounterMode_Down)
		{
			EncoderOverflowCnt++;/*±àÂëÆ÷¼ÆÊýÖµ[ÏòÉÏ]Òç³ö*/
		}
		else
		{
			EncoderOverflowCnt--;/*±àÂëÆ÷¼ÆÊýÖµ[ÏòÏÂ]Òç³ö*/
		}
	}
	TIM_ClearITPendingBit(TIM4,TIM_IT_Update);  //Çå³ýÖÐ¶Ï±êÖ¾Î»
}
int Read_Speed(void)
{
  int Encoder_TIM;    

		Encoder_TIM= (short)TIM4 -> CNT;  
		TIM4 -> CNT=0;	

		return Encoder_TIM;
}

二、电机PID控制算法

记得PID参数自己调一下哦

/**************************************************************************
º¯Êý¹¦ÄÜ£ºÔöÁ¿PI¿ØÖÆÆ÷
Èë¿Ú²ÎÊý£º±àÂëÆ÷²âÁ¿Öµ£¬Ä¿±êËÙ¶È
·µ»Ø  Öµ£ºµç»úPWM
¸ù¾ÝÔöÁ¿Ê½ÀëÉ¢PID¹«Ê½ 
pwm+=Kp[e£¨k£©-e(k-1)]+Ki*e(k)+Kd[e(k)-2e(k-1)+e(k-2)]
e(k)´ú±í±¾´ÎÆ«²î 
e(k-1)´ú±íÉÏÒ»´ÎµÄÆ«²î  ÒÔ´ËÀàÍÆ 
pwm´ú±íÔöÁ¿Êä³ö
ÔÚÎÒÃÇµÄËÙ¶È¿ØÖÆ±Õ»·ÏµÍ³ÀïÃæ£¬Ö»Ê¹ÓÃPI¿ØÖÆ
pwm+=Kp[e£¨k£©-e(k-1)]+Ki*e(k)
**************************************************************************/
int Incremental_P(int Encoder,int Target)
{ 	
  static int pwmout=0,last_error=0,last_last_error=0;
  int error =Target  - Encoder;
	int d_error=error-last_error;
  int dd_error = -2*last_error+error+last_last_error; 
//	//ËÀÇøãÐÖµ	
//	if((error>-cu_error_zone)&&(error<cu_error_zone))
//	{
//		error=0;
//		d_error=0;
//		dd_error=0;
//	}
  pwmout+=P*d_error/10 +I*error/10+D*dd_error/10;
  last_last_error=last_error;
  last_error = error;
	if(pwmout > 1000) pwmout = 1000;
	if(pwmout <=0) pwmout = 0;
  return pwmout;
}

/**************************************************************************
º¯Êý¹¦ÄÜ£ºÔöÁ¿PI¿ØÖÆÆ÷
Èë¿Ú²ÎÊý£º±àÂëÆ÷²âÁ¿Öµ£¬Ä¿±êËÙ¶È
·µ»Ø  Öµ£ºµç»úPWM
¸ù¾ÝÔöÁ¿Ê½ÀëÉ¢PID¹«Ê½ 
pwm+=Kp[e£¨k£©-e(k-1)]+Ki*e(k)+Kd[e(k)-2e(k-1)+e(k-2)]
e(k)´ú±í±¾´ÎÆ«²î 
e(k-1)´ú±íÉÏÒ»´ÎµÄÆ«²î  ÒÔ´ËÀàÍÆ 
pwm´ú±íÔöÁ¿Êä³ö
ÔÚÎÒÃÇµÄËÙ¶È¿ØÖÆ±Õ»·ÏµÍ³ÀïÃæ£¬Ö»Ê¹ÓÃPI¿ØÖÆ
pwm+=Kp[e£¨k£©-e(k-1)]+Ki*e(k)
**************************************************************************/
int Incremental_PI (int Encoder,int Target)
{ 	
  float KP = 0;
	float KI = 0;
	float KD = 0;
  static int pwmout=0,last_error=0,last_last_error=0;
  int error =Target  - Encoder;
	int d_error=error-last_error;
  int dd_error = -2*last_error+error+last_last_error; 
  pwmout+=KP*d_error/10 +KI*error/10+KD*dd_error/10;
  last_last_error=last_error;
  last_error = error;
	if(pwmout > 1000) pwmout = 1000;
	if(pwmout <=0) pwmout = 0;
  return pwmout;
}

/**************************************************************************
º¯Êý¹¦ÄÜ£ºÎ»ÖÃÊ½PID¿ØÖÆÆ÷
Èë¿Ú²ÎÊý£º±àÂëÆ÷²âÁ¿Öµ£¬Ä¿±êÎ»ÖÃ
·µ»Ø  Öµ£ºµç»úPWM
¸ù¾ÝÔöÁ¿Ê½ÀëÉ¢PID¹«Ê½ 
pwm+=Kp[e£¨k£©+Ki*e(k)+Kd[e(k)-e(k-1)]
e(k)´ú±í±¾´ÎÆ«²î 
e(k-1)´ú±íÉÏÒ»´ÎµÄÆ«²î  ÒÔ´ËÀàÍÆ 
pwm´ú±íÔöÁ¿Êä³ö
pwm+=Kp[e£¨k£©-e(k-1)]+Ki*e(k)
**************************************************************************/
int Incremental_PID (int Encoder,int Target)
{ 	

  static int pwmout=0,last_error=0,I_error=0;
	int error=Target-Encoder;
		int d_error=error-last_error;	
//È¡¾ø¶ÔÖµ
//	if(error>0)error=error;
//	else error=-error;
//ËÀÇøãÐÖµ	
	if((error>-lo_error_zone)&&(error<lo_error_zone))
	{
		error=0;
		I_error=0;
		d_error=0;
	}
//»ý·ÖÏî¡¢»ý·Ö·ÖÀë½Ï´óÆ«²îÊ±È¥µô»ý·Ö×÷ÓÃ
	if(error>lo_integral_start && error<lo_integral_max)
	{
		I_error+=error;
		if(I_error>lo_integral_max)
		{
			I_error=lo_integral_max;
		}
		else if(I_error<-lo_integral_max)
		{
			I_error=-lo_integral_max;
		}
	}
	//I_error+=error;
//Î»ÖÃÊ½PIDËã·¨
  pwmout+=KPP*error/10 +KII*I_error/10+KDD*d_error/10;
//Îó²î´«µÝ
	last_error = error;
//PWMÏÞ·ù
	if(pwmout > 1000) pwmout = 1000;
	if(pwmout <=0) pwmout = 0;
//·µ»Øµ±Ç°Êµ¼ÊÖµ
  return pwmout;
}

三、电机PWM输出

PWM占空比输出脉冲

#include "pwm.h"
#include "led.h"

//PWMÊä³ö³õÊ¼»¯
//arr£º×Ô¶¯ÖØ×°Öµ
//psc£ºÊ±ÖÓÔ¤·ÖÆµÊý
void PWM_Init(u16 arr,u16 psc)
{  
	GPIO_InitTypeDef GPIO_InitStruct;
	TIM_TimeBaseInitTypeDef TIM_TimeBaseInitStruct;
	TIM_OCInitTypeDef TIM_OCInitStruct;
	
	RCC_APB2PeriphClockCmd(RCC_APB2Periph_GPIOA | RCC_APB2Periph_TIM1 | RCC_APB2Periph_AFIO,ENABLE);//¿ªÆôÊ±ÖÓ
	
	GPIO_InitStruct.GPIO_Mode=GPIO_Mode_AF_PP;//³õÊ¼»¯GPIO--PA8¡¢PA11Îª¸´ÓÃÍÆÍìÊä³ö
	GPIO_InitStruct.GPIO_Pin=GPIO_Pin_8 |GPIO_Pin_11;
	GPIO_InitStruct.GPIO_Speed=GPIO_Speed_50MHz;
	GPIO_Init(GPIOA,&GPIO_InitStruct);
	
	TIM_TimeBaseStructInit(&TIM_TimeBaseInitStruct);//³õÊ¼»¯¶¨Ê±Æ÷¡£
	TIM_TimeBaseInitStruct.TIM_ClockDivision=TIM_CKD_DIV1;
	TIM_TimeBaseInitStruct.TIM_CounterMode=TIM_CounterMode_Up;
	TIM_TimeBaseInitStruct.TIM_Period=arr;
	TIM_TimeBaseInitStruct.TIM_Prescaler=psc;
	TIM_TimeBaseInit(TIM1,&TIM_TimeBaseInitStruct);/*¡¾2¡¿*///TIM2
	
	TIM_OCInitStruct.TIM_OCMode=TIM_OCMode_PWM1;//³õÊ¼»¯Êä³ö±È½Ï
	TIM_OCInitStruct.TIM_OCPolarity=TIM_OCPolarity_High;
	TIM_OCInitStruct.TIM_OutputState=TIM_OutputState_Enable;
	TIM_OCInitStruct.TIM_Pulse=0;
	TIM_OC1Init(TIM1,&TIM_OCInitStruct);
	TIM_OC4Init(TIM1,&TIM_OCInitStruct);
	
	TIM_CtrlPWMOutputs(TIM1,ENABLE);//¸ß¼¶¶¨Ê±Æ÷×¨Êô--MOEÖ÷Êä³öÊ¹ÄÜ
	
	TIM_OC1PreloadConfig(TIM1,TIM_OCPreload_Enable);/*¡¾3¡¿*///ENABLE//OC1Ô¤×°ÔØ¼Ä´æÆ÷Ê¹ÄÜ
	TIM_OC4PreloadConfig(TIM1,TIM_OCPreload_Enable);//ENABLE//OC4Ô¤×°ÔØ¼Ä´æÆ÷Ê¹ÄÜ
	TIM_ARRPreloadConfig(TIM1,ENABLE);//TIM1ÔÚARRÉÏÔ¤×°ÔØ¼Ä´æÆ÷Ê¹ÄÜ
	
	TIM_Cmd(TIM1,ENABLE);//¿ª¶¨Ê±Æ÷¡£
 

}

void Motor_Init(void)
{
	GPIO_InitTypeDef GPIO_InitStruct;
	RCC_APB2PeriphClockCmd(RCC_APB2Periph_GPIOA|RCC_APB2Periph_GPIOC,ENABLE);//¿ªÆôÊ±ÖÓ
	
	GPIO_InitStruct.GPIO_Mode=GPIO_Mode_Out_PP;//³õÊ¼»¯GPIO--PB12¡¢PB13¡¢PB14¡¢PB15ÎªÍÆÍìÊä³ö
	GPIO_InitStruct.GPIO_Pin=GPIO_Pin_6;
	GPIO_InitStruct.GPIO_Speed=GPIO_Speed_50MHz;
	GPIO_Init(GPIOA,&GPIO_InitStruct);
	//GPIO_SetBits(GPIOA,GPIO_Pin_8);
	
	GPIO_InitStruct.GPIO_Mode=GPIO_Mode_Out_PP;//³õÊ¼»¯GPIO--PB12¡¢PB13¡¢PB14¡¢PB15ÎªÍÆÍìÊä³ö
	GPIO_InitStruct.GPIO_Pin=GPIO_Pin_13;
	GPIO_InitStruct.GPIO_Speed=GPIO_Speed_50MHz;
	GPIO_Init(GPIOC,&GPIO_InitStruct);
	GPIO_SetBits(GPIOC,GPIO_Pin_13);
	
	
}

四、双闭环速度环在内、位置环在外

单闭环正转速度环

		Motor_pwm=Incremental_PI(speed,set_speed);			//»ñÈ¡PID¼ÆËãµÄPWMÖµ
		TIM_SetCompare1(TIM1,Motor_pwm);								//Êä³öPWMÖµ
		LED0=0;
		LED1=1;

单闭环正转位置环

		Motor_pwm=Incremental_PID(position,set_potision);			//»ñÈ¡PID¼ÆËãµÄPWMÖµ
		TIM_SetCompare1(TIM1,Motor_pwm);								//Êä³öPWMÖµ
		LED0=0;
		LED1=1;

双闭环正转位置环+速度环

		if((t++%2) == 0)
		{
			Motor_pwm=Incremental_PID(position,set_potision);			//»ñÈ¡PID¼ÆËãµÄPWMÖµ
		}
		Motor_pwm=Incremental_PI(speed,Motor_pwm);			//»ñÈ¡PID¼ÆËãµÄPWMÖµ
		TIM_SetCompare1(TIM1,Motor_pwm);								//Êä³öPWMÖµ
		LED0=0;
		LED1=1;

总结

本文仅仅简单介绍了软件驱动方面的配置，评论区欢迎讨论。

Kisorge

关注

3
点赞
踩
37

收藏

觉得还不错? 一键收藏
打赏
4
评论
【电机控制】小白从零开始：STM32双闭环（速度环、位置环）电机控制（软件篇）

小白从零开始：STM32双闭环（速度环、位置环）电机控制（软件篇）杭州研究生手把手教你搞不定STM32使用工具：1.语言：C语言2.代码编译：KEIL5、3.代码烧录：FLYMCU提示：以下是本篇文章正文内容，下面案例可供参考本文仅仅简单介绍了软件驱动方面的配置，评论区欢迎讨论。
复制链接

扫一扫