Finally got the range finder working.

Was reading from PB7 but TIM4 was hooked to PB6 :(