工作中經常遇到需要連續測速的應用，擔心編碼器“抖動”，導致速度測不準

一. 簡單原理

      編碼器計數  ---->  數值取樣  ----> 差分  ----> 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. 計算速度

    結合編碼器一圈的脈衝數，可以換算成速度   如角頻率，圈每分，圈每秒等