MC9S08QG8单片机实现单位时间捕获脉冲次数计数

最近正在做玩具智能车，不可避免的要用到单片机的捕获功能。
现想通过MC9S08QG8单片机实现单位时间捕获脉冲次数计数，来计算小车的速度。
问题卡在了捕获功能的实现上。

所用单片机：MC9S08QG8 8位机
实现功能：单位时间（1ms）测量捕获的脉冲个数

下面是我的程序，调了好长时间了，总是达不到目的，请大家帮我看一看

//===================================================================
// 程序说明
//利用HCS908_QG8 单片机开发板的TPM模块的输入捕获功能实现对外来信号
//上升沿的捕获功能,捕获时间设定为1ms,记录这段时间内捕获的脉冲个数
//单片机第8脚定义为输入捕获模式
//===================================================================
//头文件
//===================================================================
#include <hidef.h> /* for EnableInterrupts macro */
#include "derivative.h" /* include peripheral declarations */
//===================================================================
//变量和函数定义
//===================================================================
unsigned long int count=0; //捕获脉冲计数
unsigned long int OverFlow=0; //溢出数
unsigned long int n=0; //
//===================================================================
//端口初始化
//===================================================================
void PortB_INIT(void)
{
PTBD = 0X00; //端口B数据寄存器初始化为0
PTBDD_PTBDD5=0; //引脚PTB5作为输入引脚
}
//===================================================================
//输入捕获初始化
//===================================================================
void TPM_INIT(void)
{
TPMMODH=0x00;//设定模数，溢出时间约1ms
TPMMODL=0x1E;

//TPMC0VH=0x9C;
//TPMC0VL=0x40;
TPMSC=0x4F;//TPM时钟源为总线时钟；分频系数为128；溢出中断使能
TPMC1SC=0x44;//通道1中断使能，输入捕获模式，上升沿捕获
// TPMCNT=0; //任意时刻对TPM1CNTH或TPM1CNTL的写操作将计数寄存器的计数值复位（清零）
}
//==================================================================
void main(void) {

PortB_INIT();
TPM_INIT();
EnableInterrupts; /* enable interrupts */
/* include your code here */
for(;;) {
__RESET_WATCHDOG(); /* feeds the dog */

} /* loop forever */
/* please make sure that you never leave main */
}
//===================================================================
//计数器溢出中断
//===================================================================
interrupt 7 void TPM_OVF_ISR(void)
{
if(TPMSC_TOF==1)
{
TPMSC_TOF=0; //清除标志
count=n; //记录此刻的捕获脉冲个数
OverFlow++;
}
}
//===================================================================
//计数器通道1事件中断
//===================================================================
interrupt 6 void TPM_CAP_ISR(void)
{
if(TPMC1SC_CH1F==1)
{
TPMC1SC_CH1F=0; //清除标志
n++;
}
}