The interrupt file in
http://kevin.org/frc/2005/ has many timers in it I used the file to do the same thing the only thing that you need to do is to get the speed from more than one gear tooth so it is more accurate. you need to make a circular array or something like that
I'm still trying to understand it (circular array) myself(thank god for mentors)