STM32电机测速（正交或者霍尔编码器）

2.2 STM32电机测速（正交或者霍尔编码器）

我们这里提供左右电机测速代码，在公众号：小白学移动机器人，发送：电机测速，即可获得源码工程下载链接。

2.2.1 实现工具

STM32单片机、带编码器的直流减速电机、Keil5、（蓝牙、串口助手）调试用

2.2.2 编码器原理

（1）编码器是什么？

编码器是一种将角位移或者角速度转换成一串电数字脉冲的旋转式传感器。编码器又分为光电编码器和霍尔编码器。

（2）编码器工作原理是什么？

霍尔编码器是有霍尔马盘和霍尔元件组成。霍尔马盘是在一定直径的圆板上等分的布置有不同的磁极。霍尔马盘与电动机同轴，电动机旋转时，霍尔元件检测输出若干脉冲信号，为判断转向，一般输出两组存在一定相位差的方波信号。示意图如下：

（3）带霍尔编码器的直流减速电机接线图

（4）测速原理：

单位时间内，根据脉冲走过的距离计算电机实际速度，这里采用5ms定时器中断。

（5）采集数据方式：

通常有两种方式，第一种软件技术直接采用外部中断进行采集，根据AB相位差的不同可以判断正负。第二种硬件技术直接使用定时器的编码器模式，这里采用第二种。也是大家常说的四倍频，提高测量精度的方法。其实就是把AB相的上升沿和下降沿都采集而已，所以1变4。自己使用外部中断方式实现就比较占用资源了，所以不建议使用。

（6）速度计算方法：

这里计算的是真实的电机轮子的物理转速

电机转动一圈的脉冲数：num1 单位：个

单位时间：t 单位：秒

单位时间内捕获的脉冲变化数：num2 单位：个 （反应电机正反转）

电机轮子半径：r 单位：m

圆周率：pi 单位：无

速度：speed 单位： mm/s
$speed = 1000*num2*(2*pi*r/num1)/t$

2.2.3 部分代码分享

主要流程：

将编码器AB相使用的引脚设置成定时器的编码器模式，我们根据TIMx->CNT寄存器数据的变化，计算出单位时间内，脉冲的变化值。然后在定时器中断服务函数中进行速度计算，然后将速度数据通过蓝牙发送到PC的串口助手，验证数据是否正确。

（1）编码器引脚配置以及测速代码

#include "encoder.h"

/**************************************************************************
函数功能：把TIM2初始化为编码器接口模式
入口参数：无
返回  值：无
**************************************************************************/
void Encoder_Init_TIM2(void)
{
	TIM_TimeBaseInitTypeDef TIM_TimeBaseStructure;  
	TIM_ICInitTypeDef TIM_ICInitStructure;  
	GPIO_InitTypeDef GPIO_InitStructure;
	RCC_APB1PeriphClockCmd(RCC_APB1Periph_TIM2, ENABLE);    //使能定时器2的时钟
	RCC_APB2PeriphClockCmd(RCC_APB2Periph_GPIOA, ENABLE);   //使能PA端口时钟

	GPIO_InitStructure.GPIO_Pin = GPIO_Pin_0|GPIO_Pin_1;	//端口配置
	GPIO_InitStructure.GPIO_Mode = GPIO_Mode_IN_FLOATING;   //浮空输入
	GPIO_Init(GPIOA, &GPIO_InitStructure);					//根据设定参数初始化GPIOA

	TIM_TimeBaseStructInit(&TIM_TimeBaseStructure);
	TIM_TimeBaseStructure.TIM_Prescaler = 0x0;              //预分频器 
	TIM_TimeBaseStructure.TIM_Period = ENCODER_TIM_PERIOD;  //设定计数器自动重装值
	TIM_TimeBaseStructure.TIM_ClockDivision = TIM_CKD_DIV1; //选择时钟分频：不分频
	TIM_TimeBaseStructure.TIM_CounterMode = TIM_CounterMode_Up;TIM向上计数  
	TIM_TimeBaseInit(TIM2, &TIM_TimeBaseStructure);
	
	TIM_EncoderInterfaceConfig(TIM2, TIM_EncoderMode_TI12, TIM_ICPolarity_Rising, TIM_ICPolarity_Rising);//使用编码器模式3
	
	TIM_ICStructInit(&TIM_ICInitStructure);
	TIM_ICInitStructure.TIM_ICFilter = 10;
	TIM_ICInit(TIM2, &TIM_ICInitStructure);
	TIM_ClearFlag(TIM2, TIM_FLAG_Update);                   //清除TIM的更新标志位
	TIM_ITConfig(TIM2, TIM_IT_Update, ENABLE);
	//Reset counter
	TIM_SetCounter(TIM2,0);
	//===============================================
	TIM2->CNT = 0x7fff;
	//===============================================
	TIM_Cmd(TIM2, ENABLE); 
}
/**************************************************************************
函数功能：把TIM4初始化为编码器接口模式
入口参数：无
返回  值：无
**************************************************************************/
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_APB2PeriphClockCmd(RCC_APB2Periph_GPIOB, ENABLE);   //使能PB端口时钟

	GPIO_InitStructure.GPIO_Pin = GPIO_Pin_6|GPIO_Pin_7;	//端口配置
	GPIO_InitStructure.GPIO_Mode = GPIO_Mode_IN_FLOATING;   //浮空输入
	GPIO_Init(GPIOB, &GPIO_InitStructure);					//根据设定参数初始化GPIOB

	TIM_TimeBaseStructInit(&TIM_TimeBaseStructure);
	TIM_TimeBaseStructure.TIM_Prescaler = 0x0;              // 预分频器 
	TIM_TimeBaseStructure.TIM_Period = ENCODER_TIM_PERIOD;  //设定计数器自动重装值
	TIM_TimeBaseStructure.TIM_ClockDivision = TIM_CKD_DIV1; //选择时钟分频：不分频
	TIM_TimeBaseStructure.TIM_CounterMode = TIM_CounterMode_Up;TIM向上计数  
	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的更新标志位
	TIM_ITConfig(TIM4, TIM_IT_Update, ENABLE);
	//Reset counter
	TIM_SetCounter(TIM4,0);
	//===============================================
	TIM4->CNT = 0x7fff;
	//===============================================
	TIM_Cmd(TIM4, ENABLE); 
}

/**************************************************************************
函数功能：读取编码器脉冲差值
入口参数：TIM_TypeDef * TIMx
返回  值：无
**************************************************************************/
s16 getTIMx_DetaCnt(TIM_TypeDef * TIMx)
{
	s16 cnt;
	cnt = TIMx->CNT-0x7fff;     //这一点默认认为，单位时间内编码器不会出现0x7fff的脉冲变化，事实上也是这样
	TIMx->CNT = 0x7fff;
	return cnt;
}

/**************************************************************************
函数功能：计算左右轮速
入口参数：int *leftSpeed,int *rightSpeed
返回  值：无
		//计算左右车轮线速度，正向速度为正值 ，反向速度为负值，速度为乘以1000之后的速度 mm/s
		//一定时间内的编码器变化值*转化率（转化为直线上的距离m）*200s（5ms计算一次） 得到 m/s *1000转化为int数据

		一圈的脉冲数：
			左：1560
			右：1040
		轮子半径：0.03m
		轮子周长：2*pi*r
		一个脉冲的距离：
			左：0.000120830m
			右：0.000181245m
		速度分辨率：
			左：0.0120m/s 12.0mm/s
			右：0.0181m/s 18.1mm/s
**************************************************************************/

void Get_Motor_Speed(int *leftSpeed,int *rightSpeed)
{
	static int leftWheelEncoderNow    = 0;
	static int rightWheelEncoderNow   = 0;
	static int leftWheelEncoderLast   = 0;
	static int rightWheelEncoderLast  = 0;	
	
	//记录本次左右编码器数据
	leftWheelEncoderNow += getTIMx_DetaCnt(TIM4);
	rightWheelEncoderNow+= getTIMx_DetaCnt(TIM2);
		
	//5ms测速    	
	*leftSpeed   = (leftWheelEncoderNow - leftWheelEncoderLast)*1000*200*0.000120830;  
	*rightSpeed  = (rightWheelEncoderNow - rightWheelEncoderLast)*1000*200*0.000181245;

	//记录上次编码器数据
	leftWheelEncoderLast  = leftWheelEncoderNow;                    
	rightWheelEncoderLast = rightWheelEncoderNow;                   
}

（2）main.c

#include "sys.h"

//====================自己加入的头文件===============================
#include "delay.h"
#include "led.h"
#include "encoder.h"
#include "usart3.h"
#include "timer.h"
#include <stdio.h>
//===================================================================

int leftSpeedNow  =0;
int rightSpeedNow =0;

int main(void)
{ 

	GPIO_PinRemapConfig(GPIO_Remap_SWJ_Disable,ENABLE);
	GPIO_PinRemapConfig(GPIO_Remap_SWJ_JTAGDisable,ENABLE);//禁用JTAG 启用 SWD
	
	MY_NVIC_PriorityGroupConfig(2);	//=====设置中断分组
	
	delay_init();	    	        //=====延时函数初始化
	LED_Init();                     //=====LED初始化    程序灯	
	
	usart3_init(9600);              //=====串口3初始化  蓝牙 发送调试信息

	Encoder_Init_TIM2();            //=====初始化编码器1接口
	Encoder_Init_TIM4();            //=====初始化编码器2接口
	
	TIM3_Int_Init(50-1,7200-1);     //=====定时器初始化 5ms一次中断
	
	while(1)
	{
		printf("L=%d,R=%d\r\n",leftSpeedNow,rightSpeedNow);
		delay_ms(15);
	} 
}

//5ms 定时器中断服务函数

void TIM3_IRQHandler(void)   //TIM3中断
{
	if(TIM_GetITStatus(TIM3, TIM_IT_Update) != RESET) //检查指定的TIM中断发生与否:TIM 中断源 
	{
		TIM_ClearITPendingBit(TIM3, TIM_IT_Update);   //清除TIMx的中断待处理位:TIM 中断源 
		
		Get_Motor_Speed(&leftSpeedNow,&rightSpeedNow);
		Led_Flash(100);
	}
}

2.2.4 总结

上面的一篇文章，我们实现的STM32单片机对直流减速电机的测速。再加上之前介绍的电机PWM控制，下一篇我们要结合前两篇的内容加上PID实现对电机速度的闭环控制。

系列文章

搭建ROS小车真的难吗？
ROS小车软件结构以及控制流程
STM32电机PWM控制
下一篇：STM32电机PID速度控制