diff --git a/src/main.c b/src/main.c
index 9f721dd..d9ccd0b 100644
--- a/src/main.c
+++ b/src/main.c
@@ -13,7 +13,7 @@
 #endif /* DEBUG */
 
 #ifndef F_CPU
-#define F_CPU 1000000UL
+#define F_CPU 1000000UL // Stock atmega328p
 #endif
 
 
@@ -37,6 +37,7 @@ int main(void)
 {
     init_leds();
     init_servos();
+    init_sonar();
 
     run_servos();
     read_temp();
diff --git a/src/sonar.c b/src/sonar.c
index 6d7fe3f..e25d684 100644
--- a/src/sonar.c
+++ b/src/sonar.c
@@ -31,7 +31,6 @@ ISR(TIMER1_OVF_vect)
 int32_t read_sonar(void)
 {
     int dist_in_cm = 0;
-    init_sonar();
     trigger_sonar();
 
     // While echo pin is low