工作中经常遇到需要连续测速的应用,担心编码器“抖动”,导致速度测不准
一. 简单原理
编码器计数 ----> 数值取样 ----> 差分 ----> FIR滤波 ----> 比例计算实际速度
二. 硬件接口
三. 差分计算
#define MAXCOUNT1 20000 //最大计数值 per 0.1s
#define ENCODER_TIM_PERIOD 39999 //计数自动重装值 值在 0 到 39999之间
/*使用100ms 定时器,定时调用该函数*/
int16_t Enc_GetCount(void)
{
static uint16_t last_count = 0;
uint16_t cur_count;
cur_count = __HAL_TIM_GetCounter(&htim2); /*获取编码计数值*/
int32_t Angle = cur_count - last_count;
if(Angle > MAXCOUNT1) /*判定上溢出*/
{
Angle -= ENCODER_TIM_PERIOD;
}
if(Angle < -MAXCOUNT1) /*判定下溢出*/
{
Angle += ENCODER_TIM_PERIOD;
}
last_count = cur_count;
return Angle; /*返回差分值*/
}
四. FIR滤波器
使用matlab 的 FDAtool工具,设计FIR滤波器
采样频率Fs = 10Hz 阶数order = 5(过长延迟增加)
导出滤波器参数
h(6) =
-0.0691 0.0296 0.5204 0.5204 0.0296 -0.0691
C语言实现的FIR滤波器计算
/*简单的FIR滤波器*/
const float h[6] = {-0.0691 ,0.0296 ,0.5204 ,0.5204 ,0.0296 ,-0.0691}
float Fir_Filter(int16_t xn)
{
static int16_t xx[6] = {0}; /*FIR 长度6*/
float ret;
int16_t i;
xx[0] = xn;
ret = xx[0] * h[0];
for(i = 5;i > 0;i--)
{
ret += h(i) * xx[i];
xx(i) = xx(i-1);
}
return ret;
}
5. 计算速度
结合编码器一圈的脉冲数,可以换算成速度 如角频率,圈每分,圈每秒等