手把手教你搭建ROS阿克曼转向小车之（霍尔编码器数据读取与速度计算）

上一篇文章已经介绍了如何驱动直流有刷电机转动起来，这篇文章讲解如何获取编码器的计数值，并且计算出速度信息。在实际的运行中，随着机器的重量不一样，电机受到的阻力就会不一样，给定同样的PWM在不同载重的情况下速度会不一样，要解决这个问题就需要引入反馈系统，使用PID进行调节，通过期望值和反馈值的信息动态的去调整PWM值，从而保证速度满足期望值的要求，而反馈值就是需要通过编码器来进行计算。

硬件介绍

控制器的编码器端口如下图所示：

其中PA0是定时器5的CH1、PA1是定时器5的CH2；PA15是定时器2的CH1、PB3是定时器2的CH2，定时器的编码器模式只能接在CH1和CH2端口上，STM32定时器的编码器模式原理需要大家自行去阅读手册，博文只讲解代码实现。

初始化代码

初始化代码在HwConfig/hw_STM32F40x.c中：

void EncoderInit(uint8_t eName_t,uint8_t mMotorType){
	GPIO_TypeDef* ENCODER_A_PORT[ENCODERn] = {STARBOT_ENCODER1_A_GPIO_PORT, STARBOT_ENCODER2_A_GPIO_PORT, STARBOT_ENCODER3_A_GPIO_PORT, STARBOT_ENCODER4_A_GPIO_PORT};
	GPIO_TypeDef* ENCODER_B_PORT[ENCODERn] = {STARBOT_ENCODER1_B_GPIO_PORT, STARBOT_ENCODER2_B_GPIO_PORT, STARBOT_ENCODER3_B_GPIO_PORT, STARBOT_ENCODER4_B_GPIO_PORT};
	TIM_TypeDef*  ENCODER_TIM[ENCODERn] = {STARBOT_ENCODER1_TIM, STARBOT_ENCODER2_TIM, STARBOT_ENCODER3_TIM, STARBOT_ENCODER4_TIM,};
	const uint16_t  ENCODER_TIR[ENCODERn] = {STARBOT_ENCODER1_TIR,STARBOT_ENCODER2_TIR,STARBOT_ENCODER3_TIR,STARBOT_ENCODER4_TIR};
	const uint32_t  ENCODER_TIM_CLK[ENCODERn] = {STARBOT_ENCODER1_TIM_CLK, STARBOT_ENCODER2_TIM_CLK, STARBOT_ENCODER3_TIM_CLK, STARBOT_ENCODER4_TIM_CLK};
	const uint16_t  ENCODER_GPIO_AF_TIM[ENCODERn] = {STARBOT_ENCODER1_GPIO_AF_TIM,STARBOT_ENCODER2_GPIO_AF_TIM,STARBOT_ENCODER3_GPIO_AF_TIM,STARBOT_ENCODER4_GPIO_AF_TIM};
	uint32_t  ENCODER_A_PORT_CLK[ENCODERn] = {STARBOT_ENCODER1_A_GPIO_CLK, STARBOT_ENCODER2_A_GPIO_CLK, STARBOT_ENCODER3_A_GPIO_CLK, STARBOT_ENCODER4_A_GPIO_CLK,};
	uint32_t  ENCODER_B_PORT_CLK[ENCODERn] = {STARBOT_ENCODER1_B_GPIO_CLK,STARBOT_ENCODER2_B_GPIO_CLK,STARBOT_ENCODER3_B_GPIO_CLK,STARBOT_ENCODER4_B_GPIO_CLK};
	uint16_t  ENCODER_A_PIN[ENCODERn] = {STARBOT_ENCODER1_A_PIN, STARBOT_ENCODER2_A_PIN, STARBOT_ENCODER3_A_PIN, STARBOT_ENCODER4_A_PIN,};
	uint16_t  ENCODER_B_PIN[ENCODERn] = {STARBOT_ENCODER1_B_PIN, STARBOT_ENCODER2_B_PIN, STARBOT_ENCODER3_B_PIN, STARBOT_ENCODER4_B_PIN};
	uint16_t  ENCODER_A_GPIO_PinSource[ENCODERn] ={STARBOT_ENCODER1_A_GPIO_PinSource,STARBOT_ENCODER2_A_GPIO_PinSource,STARBOT_ENCODER3_A_GPIO_PinSource,STARBOT_ENCODER4_A_GPIO_PinSource};
	uint16_t  ENCODER_B_GPIO_PinSource[ENCODERn] ={STARBOT_ENCODER1_B_GPIO_PinSource,STARBOT_ENCODER2_B_GPIO_PinSource,STARBOT_ENCODER3_B_GPIO_PinSource,STARBOT_ENCODER4_B_GPIO_PinSource};

	TIM_TimeBaseInitTypeDef TIM_TimeBaseStructure;  
	TIM_ICInitTypeDef TIM_ICInitStructure;  
	NVIC_InitTypeDef NVIC_InitStructure;
	GPIO_InitTypeDef GPIO_InitStructure;

	RCC_APB1PeriphClockCmd(ENCODER_TIM_CLK[eName_t], ENABLE);  
	RCC_AHB1PeriphClockCmd(ENCODER_A_PORT_CLK[eName_t]|ENCODER_B_PORT_CLK[eName_t], ENABLE);

	GPIO_InitStructure.GPIO_Pin = ENCODER_A_PIN[eName_t];
	GPIO_InitStructure.GPIO_Speed = GPIO_Speed_100MHz;
	GPIO_InitStructure.GPIO_Mode = GPIO_Mode_AF;
	GPIO_InitStructure.GPIO_OType = GPIO_OType_OD;
	GPIO_InitStructure.GPIO_PuPd = GPIO_PuPd_UP;
	GPIO_Init(ENCODER_A_PORT[eName_t], &GPIO_InitStructure);  

	GPIO_InitStructure.GPIO_Pin = ENCODER_B_PIN[eName_t]; 
	GPIO_InitStructure.GPIO_Speed = GPIO_Speed_100MHz;
	GPIO_InitStructure.GPIO_Mode = GPIO_Mode_AF;
	GPIO_InitStructure.GPIO_OType = GPIO_OType_OD;
	GPIO_InitStructure.GPIO_PuPd = GPIO_PuPd_UP;
	GPIO_Init(ENCODER_B_PORT[eName_t], &GPIO_InitStructure);  

	GPIO_PinAFConfig(ENCODER_A_PORT[eName_t],ENCODER_A_GPIO_PinSource[eName_t],ENCODER_GPIO_AF_TIM[eName_t]);  
	GPIO_PinAFConfig(ENCODER_B_PORT[eName_t],ENCODER_B_GPIO_PinSource[eName_t],ENCODER_GPIO_AF_TIM[eName_t]);   

	TIM_TimeBaseStructInit(&TIM_TimeBaseStructure);

	TIM_TimeBaseStructure.TIM_Prescaler = 0x0; 																					// No prescaling //不分频
	if(ENCODER1 == eName_t || ENCODER2 == eName_t){
		TIM_TimeBaseStructure.TIM_Period = 0xffffffff;  																		//设定计数器自动重装值
	} else {
		TIM_TimeBaseStructure.TIM_Period = 0xffff;  																			//设定计数器自动重装值
	}

	TIM_TimeBaseStructure.TIM_ClockDivision = TIM_CKD_DIV1; 																	//选择时钟分频：不分频
	TIM_TimeBaseStructure.TIM_CounterMode = TIM_CounterMode_Up; 																//TIM向上计数    
	TIM_TimeBaseInit(ENCODER_TIM[eName_t], &TIM_TimeBaseStructure);  															//初始化定时器
	TIM_EncoderInterfaceConfig(ENCODER_TIM[eName_t], TIM_EncoderMode_TI12, TIM_ICPolarity_Rising, TIM_ICPolarity_Rising);		//使用编码器模式3

	TIM_ICStructInit(&TIM_ICInitStructure);
	TIM_ICInitStructure.TIM_ICFilter = 6;
	TIM_ICInit(ENCODER_TIM[eName_t], &TIM_ICInitStructure);
	TIM_SetCounter(ENCODER_TIM[eName_t],0);
	TIM_ITConfig(ENCODER_TIM[eName_t], TIM_IT_Update, ENABLE);
	TIM_ClearITPendingBit(ENCODER_TIM[eName_t], TIM_IT_Update);															//清除TIM的更新标志位
	TIM_ClearFlag(ENCODER_TIM[eName_t], TIM_FLAG_Update);																//清除TIM的更新标志位
	TIM_Cmd(ENCODER_TIM[eName_t], ENABLE);

	NVIC_InitStructure.NVIC_IRQChannel=ENCODER_TIR[eName_t]; 
	NVIC_InitStructure.NVIC_IRQChannelPreemptionPriority=6;
	NVIC_InitStructure.NVIC_IRQChannelSubPriority=0; 
	NVIC_InitStructure.NVIC_IRQChannelCmd=ENABLE;
	NVIC_Init(&NVIC_InitStructure);
}

定时器2和定时器5是32位定时器，所以设置计数为0xFFFFFFFF，然后我们还需要在溢出中断里对溢出次数进行处理，和需要一个定时器定时去计算单位时间内编码器的增量，然后换算为速度值。

溢出处理

溢出处理的代码在APP/moveBase_Task.cpp中：

void TIM2_IRQHandler(void)
{ 		    		  			    
    if(TIM2->SR&0X0001)//溢出中断
	{ 
		if((TIM2->CR1 & 0x10) == 0x10){
			OverEnc2--;
		}else{
			OverEnc2++;
		}		
	}				   
	TIM2->SR&=~(1<<0);//清除中断标志位 	    
}
void TIM5_IRQHandler(void)
{ 		    		  			    
	if(TIM5->SR&0X0001)//溢出中断
	{    	
		if((TIM5->CR1 & 0x10) == 0x10){
		    OverEnc1--;
		}else{
			OverEnc1++;
		}			
	}				   
	TIM5->SR&=~(1<<0);//清除中断标志位 	    
}

需要判断是向上溢出还是向下溢出，然后对溢出次数进行处理，处理完溢出后我们还需要一个定时器来计算他的单位时间内的增量，博主使用的是定时器6。

速度计算定时器初始化

定时器6的初始化代码在HwConfig/hw_STM32F40x.c中：

void BaseBoard_TIM6_Init(void){	//10ms
	TIM_TimeBaseInitTypeDef TIM_TimeBaseInitStructure;
	NVIC_InitTypeDef NVIC_InitStructure;
	//84MHz
	RCC_APB1PeriphClockCmd(RCC_APB1Periph_TIM6,ENABLE);  
	
	TIM_TimeBaseInitStructure.TIM_Period = 83; 	
	TIM_TimeBaseInitStructure.TIM_Prescaler=9999;  
	TIM_TimeBaseInitStructure.TIM_CounterMode=TIM_CounterMode_Up; 
	TIM_TimeBaseInitStructure.TIM_ClockDivision=TIM_CKD_DIV1; 
	
	TIM_TimeBaseInit(TIM6,&TIM_TimeBaseInitStructure);
	
	TIM_ITConfig(TIM6,TIM_IT_Update,ENABLE); 
	TIM_Cmd(TIM6,ENABLE); 
	
	NVIC_InitStructure.NVIC_IRQChannel=TIM6_DAC_IRQn; 
	NVIC_InitStructure.NVIC_IRQChannelPreemptionPriority=0x00;
	NVIC_InitStructure.NVIC_IRQChannelSubPriority=0x00; 
	NVIC_InitStructure.NVIC_IRQChannelCmd=ENABLE;
	NVIC_Init(&NVIC_InitStructure);
}

这里给定定时器的周期是10ms，即100HZ，大家可以根据自己的需要进行修改，然后在定时器6的中断函数中对编码器数据进行处理。

编码器数据处理

编码器数据处理的代码在APP/moveBase_Task.cpp中：

void TIM6_DAC_IRQHandler(void)
{   
	if(TIM_GetITStatus(TIM6,TIM_IT_Update)==SET) {
		encPIDCnt++;
		if(encPIDCnt >= DELTA_CNT){  // DELTA_CNT = 10
		    编码器采样周期 10 x 10ms = 100ms
		    if(MotorType_t != M_CAN_1_2){					
    		    gCurrentEnc1 = (int64_t)(((TIM5 -> CNT)) + OverEnc1*0xffffffff);
				gCurrentEnc2 = (int64_t)(((TIM2 -> CNT)) + OverEnc2*0xffffffff);
						
				if(lastCurrentEnc1 == 0){lastCurrentEnc1 = gCurrentEnc1;}
				delta_ticks_1 = gCurrentEnc1 - lastCurrentEnc1;
				lastCurrentEnc1 = gCurrentEnc1;
				if(lastCurrentEnc2 == 0){lastCurrentEnc2 = gCurrentEnc2;}
				delta_ticks_2 = gCurrentEnc2 - lastCurrentEnc2;
				lastCurrentEnc2 = gCurrentEnc2;
												
				Motor_Rpm.MotorEncoder1 += delta_ticks_1;
				Motor_Rpm.MotorEncoder2 += delta_ticks_2;
				Motor_Rpm.Current_Rpm1 = (delta_ticks_1*600/Time_counts_per_rev);
				Motor_Rpm.Current_Rpm2 = (delta_ticks_2*600/Time_counts_per_rev);
			}
		    moveBase_Manage();
		    encPIDCnt = 1;
		}
	}	
	TIM_ClearITPendingBit(TIM6,TIM_IT_Update);  				
}

rpm的计算公式为：分钟内编码器的增量/输出轴转一圈编码器的计数值，我们这里是100ms计算一次，因此单位时间要进行转换：1min = 60s=60000ms 所以需要乘上600就可以得到RPM值。如果文章有错误欢迎大家及时纠正，感谢大家的支持