请问一下如何对STM32正交编码的程序进行测速呢

                                                               STM32正交编码测速
      程序的大致思路如下:两个定时器配置为编码器模式，用于小车的两个轮子编码脉冲计数，计数器向上或向下计数溢出，均在二者的中断函数中记录记录。还有一个定时器用作计时用，规定时间内进入中断，在中断函数中对数据进行处理。我用的光电码盘是100线的，在选择的计数模式下，转一圈产生400个计数脉冲。程序如下：
double first_cnt,second_cnt,encoder_timer_overflow_sample;
static volatile double encoder_timer_overflow;
double rotor_speed = 0;
unsigned char i = 0;

//first_cnt是第一次读计数器的值，second_cnt是第二次读计数器的值，encoder_timer_overflow记录计数器的溢出次数（不管是向上溢出还是向下溢出）
int main(void)
{
Myusart_Init();
Encoder_Init();

first_cnt = TIM_GetCounter(ENCODER_TIMER); //第一次读取编码器计数值
encoder_timer_overflow = 0;//初始时令编码器计数溢出次数为零，认为一个处理周期内其值小于double类的极值
while(1);

}


double Get_Rotor_Speed(void)//double Get_Rotor_Speed()
{
double delta_cnt;//记录前后读取计数器计数器计数的差值
double w_rotor,line_speed,circle_number = 0;

second_cnt = TIM_GetCounter( ENCODER_TIMER );//读取编码器计数值
encoder_timer_overflow_sample = encoder_timer_overflow;//从encoder_timer_overflow中读取溢出次数

if ( (ENCODER_TIMER->CR1 & TIM_CounterMode_Down) == TIM_CounterMode_Down )
{
// encoder timer down-counting  编码器是向下计数，
  delta_cnt = ( second_cnt - first_cnt - encoder_timer_overflow_sample * (4 * ENCODER_PPR) );
// a negetive value计算前后两次读取的计数总差值
}
else
{
//encoder timer up-counting编码器向上计数
  delta_cnt = ( second_cnt - first_cnt + encoder_timer_overflow_sample  * (4 * ENCODER_PPR) );
  // a positive value            
}

first_cnt = second_cnt;//保存第二次的读取值，以便下一次使用
encoder_timer_overflow = 0;//溢出次数清零

circle_number = delta_cnt / 400.0 / 98.777946;//计算两次读取时间内车轮转了多少圈

//400  :  the count value of CNT for rotor  rotate a circle
//98.777946  :  the decrease speed rate of motor 减速箱的减速比，delta是转子所转的圈数

w_rotor = ( circle_number * 2 * 3.141592 ) / 0.03;//计算角速度，2*pi*转的圈数/计数时间（为0.03s）

//the wheel's w_rotor ,calculate time is 1 minute
//circle_number * 2 * 3.141592    delta_angle  by radian
//w_rotor      unit:  degree by radian per second

line_speed = ( w_rotor * 64.68 / 2.0 ) / 10.0;//计算线速度，v=w*r车轮直径64.68mm,除以10转化为cm

//the wheel's line_speed, unit: cm per seconds
//64.68 / 2.0 mm: radius of wheel
//printf ( " w_rotor=%8lf       line_speed =%8lfrn ",w_rotor,line_speed );

return line_speed;
}
void TIM3_IRQHandler(void)//定时器3定时器0.03秒，在中断函数中进行平均值滤波。
{   
  if ( i<8 )
{
  rotor_speed += Get_Rotor_Speed();//assume rotor_speed will not larger than double_max
  i++;
}
else
{
  rotor_speed /= i; //读取8次值，然后取平均值
  printf ( " a%8lfrn ",rotor_speed  * 100 );//a, +500 used for OSC
  //clear to 0 for next use
  i = 0;
  rotor_speed = 0;//清零，以备下次使用
}

/* Clear the interrupt pending flag */
TIM_ClearFlag(TIM3,TIM_FLAG_Update);
}



完