diff --git a/include/sonar.h b/include/sonar.h
new file mode 100644
index 0000000..03a1199
--- /dev/null
+++ b/include/sonar.h
@@ -0,0 +1,45 @@
+#ifndef SONAR_H
+#define SONAR_H
+
+#include "common.h"
+
+// Trigger
+#define TRIG_DDR    DDRC
+#define TRIG_PORT   PORTC
+#define TRIG_PIN    PINC
+#define TRIG_BIT    PC4
+
+// Echo
+#define ECHO_DDR    DDRC
+#define ECHO_PORT   PORTC
+#define ECHO_PIN    PINC
+#define ECHO_BIT    PC5
+
+#define SPEED_OF_SOUND      343 // (m/s)
+#define US_PER_CM           58 // Time for sound to travel distance of 1cm
+#define MAX_SONAR_RANGE     8 // Trigger + echo (m)
+#define DELAY_BETWEEN_TESTS	500 // Timeout for return signal
+#define TIMER_MAX           65535 // Depends on the timer used
+
+#define CYCLES_PER_US   F_CPU / 1000000
+#define SONAR_TIMEOUT 	(F_CPU * MAX_SONAR_RANGE) / SPEED_OF_SOUND
+
+#define TRIG_ERROR      -1
+#define ECHO_ERROR      -2
+
+#define TRIG_INPUT_MODE()   CLR(TRIG_DDR, TRIG_BIT)
+#define TRIG_OUTPUT_MODE() 	SET(TRIG_DDR, TRIG_BIT)
+#define TRIG_LOW()          CLR(TRIG_PORT, TRIG_BIT)
+#define TRIG_HIGH()         SET(TRIG_PORT, TRIG_BIT)
+
+#define ECHO_INPUT_MODE()   CLR(ECHO_DDR, ECHO_BIT)
+#define ECHO_OUTPUT_MODE()  SET(ECHO_DDR, ECHO_BIT)
+#define ECHO_LOW()          CLR(ECHO_PORT, ECHO_BIT)
+#define ECHO_HIGH()         SET(ECHO_PORT, ECHO_BIT)
+
+
+void init_sonar(void);
+void trigger_sonar(void);
+unsigned int read_sonar(void);
+
+#endif /* SONAR_H */
diff --git a/src/sonar.c b/src/sonar.c
new file mode 100644
index 0000000..4f15e14
--- /dev/null
+++ b/src/sonar.c
@@ -0,0 +1,61 @@
+#include <avr/interrupt.h>
+#include <util/delay.h>
+#include "sonar.h"
+
+volatile uint32_t overflow_counter = 0;
+volatile uint32_t trig_counter = 0;
+volatile uint32_t no_of_cycles = 0;
+
+void init_sonar()
+{
+    TRIG_OUTPUT_MODE();
+    ECHO_INPUT_MODE();
+}
+
+void trigger_sonar()
+{
+    TRIG_LOW();
+    _delay_us(1);
+    TRIG_HIGH();
+    _delay_us(12);
+    TRIG_LOW();
+    _delay_us(1);
+}
+
+ISR(TIMER1_OVF_vect)
+{
+    ++overflow_counter;
+    TCNT1 = 0;
+}
+
+unsigned int read_sonar()
+{
+    int dist_in_cm = 0;
+    init_sonar();
+    trigger_sonar();
+
+    // While echo pin is low
+    while (!(CHK(ECHO_PIN, ECHO_BIT))) {
+        if (++trig_counter > SONAR_TIMEOUT)
+            return TRIG_ERROR; // Received no response from echo
+    }
+
+    TCNT1 = 0;
+    TCCR1B |= (1 << CS10);
+    TIMSK1 |= (1 << TOIE1);
+    overflow_counter = 0;
+    sei();
+
+    // While echo pin is high
+    while (CHK(ECHO_PIN, ECHO_BIT)) {
+        if (((overflow_counter * TIMER_MAX) + TCNT1) > SONAR_TIMEOUT)
+            return ECHO_ERROR; // No echo within sonar range
+    }
+
+    TCCR1B = 0x00;
+    cli();
+    no_of_cycles = ((overflow_counter * TIMER_MAX) + TCNT1);
+    dist_in_cm = (no_of_cycles / (US_PER_CM * CYCLES_PER_US));
+
+    return dist_in_cm;
+}