diff --git a/robot/src/sonar.c b/robot/src/sonar.c
index 801009c..f5bd653 100644
--- a/robot/src/sonar.c
+++ b/robot/src/sonar.c
@@ -5,7 +5,7 @@
 #include "led.h"
 
 volatile uint32_t trig_counter;
-uint32_t no_of_ticks;
+volatile uint32_t no_of_ticks;
 
 void init_sonar(void)
 {
@@ -36,15 +36,19 @@ uint16_t read_sonar(void)
     sei();
     // Count cycles until ECHO goes low
     while (CHK(ECHO_PIN, ECHO_BIT)) {
-        ++no_of_ticks;
-        if (no_of_ticks > SONAR_TIMEOUT) {
+        if (no_of_ticks + TCNT1 > SONAR_TIMEOUT) {
             return SONAR_ERROR;
         }
     }
     TCCR1B = 0x00;
     cli();
 
+    no_of_ticks += TCNT1;
 
     return (uint16_t)(no_of_ticks / (CYCLES_PER_US * 58));
 }
 
+ISR(TIMER1_OVF_vect)
+{
+    no_of_ticks += TIMER_MAX;
+}