STM32F4单编码电机位置控制

提示:文章写完后,目录可以自动生成,如何生成可参考右边的帮助文档


1. 霍尔编码器原理

霍尔编码器是一种利用霍尔效应来检测电机转子位置的传感器装置。其基本原理是,当电机的转子旋转时,内置的霍尔传感器能够检测到转子磁场的变化。每个霍尔传感器会根据转子的磁场变化产生一个电压信号,这些信号通常以脉冲的形式输出。通过读取这些脉冲信号,控制系统可以确定电机轴的旋转角度、速度以及旋转方向。霍尔编码器的优点在于其结构简单、耐用性强,广泛应用于直流无刷电机(BLDC)的位置反馈和转速控制。

2. 硬件部署

2.1 硬件

  • STM32F407 主控板
  • TB6612电机驱动板
  • 单编码器电机
    在这里插入图片描述
  • 丝杠导轨(执行机构,根据需要选择)
    在这里插入图片描述

2.2 接线

  • 编码器接线方式


图源 :GA12-N20减速马达

  • TB6612接线方式
    TB6612电机驱动芯片引脚说明
    在这里插入图片描述
    在这里插入图片描述
    芯片引脚图与表,图源WheelTec TB6612 电机驱动
    电机输入端输入真值表
    在这里插入图片描述
    图源WheelTec TB6612 电机驱动

  • 主控接线方式(STM32对应电机驱动及编码器接线)

STM32引脚功能对应
PD12/TIM41编码器
PD13/TIM42编码器
PA2/TIM23编码电机PWM A
PD14编码电机IN1
PD15编码电机IN2

3. STM32资源分配

TIM4——通道1&通道2——编码器读取;
TIM2——通道3——电机PWM驱动信号;

4. STM32F4代码实现

【说明】该部分代码基于RTthread,裸板使用可以直接取出rt定时器中的循环放入新的定时器中。主函数参考STM32控制编码器电机实现【速度闭环控制】与【位置闭环控制】

4.1 定时器定义:

time.c

这部分代码定义了TIM2和TIM4两个定时器,分别用于PWM输出和编码器的读取;


#include "stm32f4xx.h"
#include "myconst.h"
#include "myfunction.h"
#include "math.h"

extern uint16_t CCR_Val;
#define ENCODER_TIM_PERIOD 65535  // 16位定时器的最大值

/*******************************************************
PWM输出定时器定义
*******************************************************/
//定时器的PWM输出引脚配置
void TIM2_GPIO_Config(void)
{
	GPIO_InitTypeDef GPIO_InitStructure;
	
	RCC_AHB1PeriphClockCmd(RCC_AHB1Periph_GPIOA, ENABLE);
	
	GPIO_InitStructure.GPIO_Mode = GPIO_Mode_AF;
	GPIO_InitStructure.GPIO_Speed = GPIO_Speed_100MHz;
	GPIO_InitStructure.GPIO_OType = GPIO_OType_PP;
	GPIO_InitStructure.GPIO_PuPd = GPIO_PuPd_UP;
	GPIO_InitStructure.GPIO_Pin = GPIO_Pin_0 | GPIO_Pin_1 | GPIO_Pin_2 | GPIO_Pin_3;
	GPIO_Init(GPIOA, &GPIO_InitStructure);
	
	GPIO_PinAFConfig(GPIOA, GPIO_PinSource0, GPIO_AF_TIM2);
	GPIO_PinAFConfig(GPIOA, GPIO_PinSource1, GPIO_AF_TIM2);
	GPIO_PinAFConfig(GPIOA, GPIO_PinSource2, GPIO_AF_TIM2);
	GPIO_PinAFConfig(GPIOA, GPIO_PinSource3, GPIO_AF_TIM2);
}

//定时器参数配置
void TIM2_Config(void)
{
	TIM_TimeBaseInitTypeDef TIM_TimeBaseStructure;
	TIM_OCInitTypeDef TIM_OCInitStructure;

	RCC_APB1PeriphClockCmd( RCC_APB1Periph_TIM2, ENABLE); 	

	//定时器4配置(输出PWM波)
	TIM_DeInit(TIM2);
	TIM_TimeBaseStructInit(&TIM_TimeBaseStructure); 
	TIM_TimeBaseStructure.TIM_Period = TOP_PWM;
	TIM_TimeBaseStructure.TIM_Prescaler = 84;
	TIM_TimeBaseStructure.TIM_ClockDivision = TIM_CKD_DIV1;
	TIM_TimeBaseStructure.TIM_CounterMode = TIM_CounterMode_Up;	
	TIM_TimeBaseInit(TIM2, &TIM_TimeBaseStructure);

	TIM_OCInitStructure.TIM_OCMode = TIM_OCMode_PWM1;
	TIM_OCInitStructure.TIM_OutputState = TIM_OutputState_Enable;
	TIM_OCInitStructure.TIM_Pulse = CCR_Val;
	TIM_OCInitStructure.TIM_OCPolarity = TIM_OCPolarity_High;	
	TIM_OC1Init(TIM2, &TIM_OCInitStructure);
	TIM_OC1PreloadConfig(TIM2, TIM_OCPreload_Enable);
	
	TIM_OCInitStructure.TIM_OutputState = TIM_OutputState_Enable;
	TIM_OCInitStructure.TIM_Pulse = CCR_Val;	
	TIM_OC2Init(TIM2, &TIM_OCInitStructure);	
	TIM_OC2PreloadConfig(TIM2, TIM_OCPreload_Enable);
	
	TIM_OCInitStructure.TIM_OutputState = TIM_OutputState_Enable;
	TIM_OCInitStructure.TIM_Pulse = CCR_Val;	
	TIM_OC3Init(TIM2, &TIM_OCInitStructure);
	TIM_OC3PreloadConfig(TIM2, TIM_OCPreload_Enable);
	
	TIM_OCInitStructure.TIM_OutputState = TIM_OutputState_Enable;
	TIM_OCInitStructure.TIM_Pulse = CCR_Val;	
	TIM_OC4Init(TIM2, &TIM_OCInitStructure);
	TIM_OC4PreloadConfig(TIM2, TIM_OCPreload_Enable);
	
	TIM_ARRPreloadConfig(TIM2, ENABLE);
	TIM_ClearFlag(TIM2, TIM_FLAG_Update);
	TIM_ITConfig(TIM2, TIM_IT_Update,ENABLE);

	//TIM_Cmd(TIM4, ENABLE);
	TIM_Cmd(TIM2, ENABLE);
}

/*******************************************************
编码器定时器定义
*******************************************************/
void Encoder_Init_TIM4(void)
{
  TIM_TimeBaseInitTypeDef TIM_TimeBaseStructure;  
  TIM_ICInitTypeDef TIM_ICInitStructure;  
  GPIO_InitTypeDef GPIO_InitStructure;
  RCC_APB1PeriphClockCmd(RCC_APB1Periph_TIM4, ENABLE);//使能定时器4的时钟
  RCC_AHB1PeriphClockCmd(RCC_AHB1Periph_GPIOD, ENABLE);//使能PD端口时钟
	
  // 配置PD12和PD13为复用功能模式
  GPIO_InitStructure.GPIO_Pin = GPIO_Pin_12 | GPIO_Pin_13; // PD12 和 PD13
  GPIO_InitStructure.GPIO_Mode = GPIO_Mode_AF;  // 复用功能模式
  GPIO_InitStructure.GPIO_Speed = GPIO_Speed_100MHz; // 高速
  GPIO_InitStructure.GPIO_OType = GPIO_OType_PP;    // 推挽输出
  GPIO_InitStructure.GPIO_PuPd = GPIO_PuPd_NOPULL;  // 无上下拉
  GPIO_Init(GPIOD, &GPIO_InitStructure);					      //根据设定参数初始化GPIOD
	
	// 配置PD12和PD13为TIM4复用功能
  GPIO_PinAFConfig(GPIOD, GPIO_PinSource12, GPIO_AF_TIM4);
  GPIO_PinAFConfig(GPIOD, GPIO_PinSource13, GPIO_AF_TIM4);
  
  // 初始化定时器4
  TIM_TimeBaseStructInit(&TIM_TimeBaseStructure);
  TIM_TimeBaseStructure.TIM_Prescaler = 0x0; // 预分频器为0
  TIM_TimeBaseStructure.TIM_Period = ENCODER_TIM_PERIOD - 1; // 设定计数器自动重装值
  TIM_TimeBaseStructure.TIM_ClockDivision = TIM_CKD_DIV1; // 不分频
  TIM_TimeBaseStructure.TIM_CounterMode = TIM_CounterMode_CenterAligned1; // 向上计数
  TIM_TimeBaseInit(TIM4, &TIM_TimeBaseStructure);

  // 配置编码器接口
  TIM_EncoderInterfaceConfig(TIM4, TIM_EncoderMode_TI12, TIM_ICPolarity_Rising, TIM_ICPolarity_Rising); // 使用编码器模式3

  // 设置输入捕获滤波器
  TIM_ICStructInit(&TIM_ICInitStructure);
  TIM_ICInitStructure.TIM_ICFilter = 10; // 设置滤波器长度
  TIM_ICInit(TIM4, &TIM_ICInitStructure);
 
  // 清除更新标志
  TIM_ClearFlag(TIM4, TIM_FLAG_Update);
  // 使能更新中断
  TIM_ITConfig(TIM4, TIM_IT_Update, ENABLE);

  // 设置计数器初始值
  TIM_SetCounter(TIM4, 10000);
  // 使能定时器4
  TIM_Cmd(TIM4, ENABLE);
}

//中断处理函数为空,清除中断标志位后结束中断
void TIM4_IRQHandler(void)
{
	rt_interrupt_enter();
if (TIM_GetITStatus(TIM4, TIM_IT_Update) != RESET) // 检查中断状态
 {
   // 清除中断标志位
   TIM_ClearITPendingBit(TIM4, TIM_IT_Update);
 }
 	rt_interrupt_leave();
}

//中断处理函数为空,清除中断标志位后结束中断
void TIM2_IRQHandler(void)
{
	rt_interrupt_enter();
if (TIM_GetITStatus(TIM2, TIM_IT_Update) != RESET) // 检查中断状态
 {
   // 清除中断标志位
   TIM_ClearITPendingBit(TIM2, TIM_IT_Update);
 }
 	rt_interrupt_leave();
}

/**************************************************************************
函数功能:单位时间读取编码器计数
入口参数:定时器
返回  值:速度值
**************************************************************************/
int Read_Encoder(u8 TIMX)//读取计数器的值
{
  int Encoder_TIM;
	switch(TIMX)
	{
	  case 2:Encoder_TIM=(short)TIM2->CNT;  break;
		case 3:Encoder_TIM=(short)TIM3->CNT;  break;
		case 4:Encoder_TIM=(short)TIM4->CNT;  break;
		default: Encoder_TIM=0;
	}
  return Encoder_TIM;
}

4.2 电机驱动IO定义

slider_moto.c

定义了两个IO用于tb6612的IN1和IN2信号控制;

#include "stm32f4xx.h"
#include "myconst.h"
#include "myfunction.h"
#include "slider_moto.h"

void slider_moto_init(void) // 初始化控制电机所需的IO
{
    GPIO_InitTypeDef GPIO_InitStruct;

    // 开启GPIOD端口时钟
    RCC_AHB1PeriphClockCmd(RCC_AHB1Periph_GPIOD, ENABLE); 

    // 配置PD14和PD15为推挽输出模式
    GPIO_InitStruct.GPIO_Mode = GPIO_Mode_OUT; 
    GPIO_InitStruct.GPIO_Pin = GPIO_Pin_14 | GPIO_Pin_15; 
    GPIO_InitStruct.GPIO_Speed = GPIO_Speed_50MHz; 
    GPIO_InitStruct.GPIO_OType = GPIO_OType_PP; // 推挽输出
    GPIO_InitStruct.GPIO_PuPd = GPIO_PuPd_NOPULL; // 无上下拉

    // 初始化GPIOD端口的PD14和PD15引脚
    GPIO_Init(GPIOD, &GPIO_InitStruct); 
}

4.3 主函数:

slieder_pos_control_app.c

创建一个RT-thread线程用于电机位置控制;
由于单编码器无法获得绝对位置,增加了一个到头检测功能用于电机保护;

#include "slider_pos_control_app.h"
#include "myconst.h"
#include "myfunction.h"
#include "math.h"

extern int Encoder;
extern int Encoder_error;
int slider_moto_amp = 7100;
int Target_position = 12000; 
float Position_KP=50,Position_KI=0,Position_KD=200;
float Bias =0,moto,Last_moto,Integral_bias =0,Last_Bias;

/*---------------------------------------------------------------------
 *  周期性控制滑块位置,同时检测滑块是否到达限位
 *  设置了一个信号量,用于保证每个控制周期电机位置
 *  增加了堵转保护,滑块运行到两端时,检测到输出和执行不相符说明堵转,执行保护
 *-------------------------------------------------------------------*/

//PWM周期性控制函数
#define SLIDER_POS_CONTROL_PERIOD (40) //unit: ms

static rt_thread_t slider_pos_control_thread;  //PWM控制线程
struct rt_semaphore slider_pos_control_sem;//下一步PWM计算的信号量
struct rt_timer slider_pos_control_timer;//imu读取定时器

//释放信号量
void slider_pos_control_thread_tmrcallback(void* parameter)
{
	slider_pos_control_sem.value=0;
	rt_sem_release(&slider_pos_control_sem);//拿到OK信号后释放信号量
}

//控制线程
static void slider_pos_control_entry(void* parameter)
{
	//主体循环
	while(1)
	{
		
		if(rt_sem_take(&slider_pos_control_sem, RT_WAITING_FOREVER) ==  RT_EOK)//判断信号量是否可以使用
		{
			
			Encoder = Read_Encoder(4);
			//PID控制
			Bias = Encoder-Target_position;                           //计算偏差
			Integral_bias += Bias;	                                 //求出偏差的积分
			moto = Position_KP*Bias+Position_KI*Integral_bias+Position_KD*(Bias-Last_Bias);   //位置式PID控制器
			
			//PWM限幅以及电机方向判定
			if (moto < -slider_moto_amp)
			{
				moto = -slider_moto_amp;
			}
			if (moto > slider_moto_amp)
			{
				moto = slider_moto_amp;
			}
			
			if (fabs(Bias) <40)
			{
				moto = 0;
			}
			else
			{
				//电机堵转保护
				if (fabs(Last_moto)>20)
				{
					if(fabs(Bias - Last_Bias)<5) //说明电机实际位置没动
					{
						rt_thread_delay(200);
						if(fabs(Bias - Last_Bias)<10)
						{
							moto = 0;
							TIM_SetCompare3(TIM2,moto);
							moto = 0;
						}
					}
				}
		 	}
			
			//执行电机控制指令
			if (moto<0)
			{
				GPIO_WriteBit(GPIOD, GPIO_Pin_14, Bit_SET); // PD14 置为高电平
				GPIO_WriteBit(GPIOD, GPIO_Pin_15, Bit_RESET);   // PD15 置为低电平
				TIM_SetCompare3(TIM2,-moto);
			}
			else
			{
				GPIO_WriteBit(GPIOD, GPIO_Pin_14, Bit_RESET); // PD14 置为低电平
				GPIO_WriteBit(GPIOD, GPIO_Pin_15, Bit_SET);   // PD15 置为高电平
				TIM_SetCompare3(TIM2,moto);
			}
			
			Last_Bias = Bias;                               //保存上一次偏差 
			Last_moto = moto;                               //保存上一次电机输出
			
		}
		rt_thread_delay(2);
	}
}
void slider_pos_control_thread_create(void)
{
	 slider_pos_control_thread = rt_thread_create("slider_pos_control",	slider_pos_control_entry, RT_NULL, 2048, 8, 10);

	 rt_timer_init(&slider_pos_control_timer,
								 "slider_pos_control_timer", 
								  slider_pos_control_thread_tmrcallback,
								  RT_NULL,
						      SLIDER_POS_CONTROL_PERIOD,  //unit:ms
								  RT_TIMER_FLAG_PERIODIC);//周期性定时器
	
	rt_sem_init(&slider_pos_control_sem, "slider_pos_control_sem", 0, RT_IPC_FLAG_PRIO);//初始化一个信号量(0)

	//rt_timer_stop(&slider_pos_control_timer);
   rt_timer_start(&slider_pos_control_timer);

	if(slider_pos_control_thread!= RT_NULL)
	{
		rt_thread_startup(slider_pos_control_thread);
	}
}

完整代码后面会在github更新

评论
添加红包

请填写红包祝福语或标题

红包个数最小为10个

红包金额最低5元

当前余额3.43前往充值 >
需支付:10.00
成就一亿技术人!
领取后你会自动成为博主和红包主的粉丝 规则
hope_wisdom
发出的红包
实付
使用余额支付
点击重新获取
扫码支付
钱包余额 0

抵扣说明:

1.余额是钱包充值的虚拟货币,按照1:1的比例进行支付金额的抵扣。
2.余额无法直接购买下载,可以购买VIP、付费专栏及课程。

余额充值