Got TOF working.  Note: DO NOT USE timer_time.  Floating point messed everything up.  Used timer_read to measured ticks.  TODO: Fix TOF_Timer so it doesn't overflow