diff --git a/examples/Makefile b/examples/Makefile
index ba22472..f050b2b 100644
--- a/examples/Makefile
+++ b/examples/Makefile
@@ -11,7 +11,7 @@ EFUSE = 0x05
 
 ## === Source files ===
 # Main C file
-MAIN = uart_stream.c
+MAIN = sonar_to_lcd.c
 # Extra C files in this folder
 LOCAL_SOURCE =
 
diff --git a/examples/sonar_to_lcd.c b/examples/sonar_to_lcd.c
new file mode 100644
index 0000000..6432182
--- /dev/null
+++ b/examples/sonar_to_lcd.c
@@ -0,0 +1,63 @@
+//
+// Basic Sonar example
+//
+
+#include <avr/io.h>
+#include <avr/interrupt.h>
+#include <util/delay.h>
+#include <stdint.h>
+#include <stdbool.h>
+
+#include "lib/arduino_pins.h"
+#include "lib/sonar.h"
+#include "lib/stream.h"
+#include "lib/lcd.h"
+
+// Sonar interrupts
+
+ISR(PCINT0_vect)
+{
+	if (sonar_handle_pci()) return;
+}
+
+ISR(PCINT1_vect, ISR_ALIASOF(PCINT0_vect));
+ISR(PCINT2_vect, ISR_ALIASOF(PCINT0_vect));
+
+ISR(TIMER1_OVF_vect)
+{
+	if (sonar_handle_t1ovf()) return;
+}
+
+
+
+void main()
+{
+	lcd_init();
+
+	// Init sonar
+	sonar_t so;
+	sonar_init(&so, A0, A1);
+
+	sei();
+
+
+	while(1) {
+		// Measure
+		sonar_start(&so);
+		while(sonar_busy);
+
+		int16_t res = sonar_result;
+
+		// Print
+		lcd_clear();
+		if (sonar_result < 0) {
+			put_str(lcd, "---");
+		} else {
+			put_i16f(lcd, res, 1); // one decimal place
+			put_str(lcd, " cm");
+		}
+
+		// a delay...
+		_delay_ms(200);
+	}
+}
diff --git a/lib/lcd.c b/lib/lcd.c
index 50f61c7..20ef4a0 100644
--- a/lib/lcd.c
+++ b/lib/lcd.c
@@ -308,6 +308,9 @@ void lcd_home()
 void lcd_clear()
 {
 	lcd_command(LCD_CLEAR);
+	_pos.x = 0;
+	_pos.y = 0;
+	_addrtype = TEXT;
 }