diff --git a/Makefile b/Makefile
index cf0ccae..6577073 100644
--- a/Makefile
+++ b/Makefile
@@ -6,7 +6,7 @@ SRC := main.c
 CC = avr-gcc
 
 INCLUDE := /usr/avr
-CFLAGS := -mmcu=atmega32u4 -DF_CPU=8000000UL -I$(INCLUDE)
+CFLAGS := -mmcu=atmega32u4 -DF_CPU=16000000UL -I$(INCLUDE)
 
 all:
 	$(CC) $(CFLAGS) -Os $(SRC) -o main.elf
diff --git a/main.c b/main.c
index ada2fc6..8d0d4d7 100644
--- a/main.c
+++ b/main.c
@@ -1,6 +1,5 @@
 #include <stdint.h>
-#define F_CPU 8000000UL // 8 MHz
-
+#include <avr/interrupt.h>
 #include <util/delay.h>
 #include <avr/io.h>
 
@@ -13,9 +12,13 @@
 #define SEG_DIG3 (1 << PF4)
 #define SEG_DIG4 (1 << PF1)
 
+#define HCSR04_TRIG (1 << PB7)
+#define HCSR04_ECHO (1 << PB6)
+
+#define SWITCH_IN (1 << PF7)
+
 void init_3461BS() {
     DDRF |= SEG_DIG1 | SEG_DIG2 | SEG_DIG3 | SEG_DIG4;
-
     PORTF &= ~(SEG_DIG1 | SEG_DIG2 | SEG_DIG3 | SEG_DIG4);
 }
 
@@ -29,7 +32,7 @@ void init_74HC595() {
 }
 
 const uint8_t digit_segments[] = {
-    0b11111110, // 0
+    0b10000001, // 0
     0b11001111, // 1
     0b10010010, // 2
     0b10000110, // 3
@@ -42,7 +45,7 @@ const uint8_t digit_segments[] = {
 };
 
 void send_serial_byte(uint8_t n) {
-    for (int i = 7; i > 0; i--) {
+    for (int i = 0; i < 8; i++) {
         if (n & (1 << i)) {
             PORTC |= SN74_SER;
         }
@@ -50,44 +53,82 @@ void send_serial_byte(uint8_t n) {
             PORTC &= ~SN74_SER;
         }
         PORTE |= SN74_SRCLK;
-        _delay_ms(100);
         PORTE &= ~SN74_SRCLK;
         PORTC &= ~SN74_SER;
-        _delay_ms(100);
     }
 
     PORTB |= SN74_RCLK;
-    _delay_ms(100);
     PORTB &= ~SN74_RCLK;
 }
 
-int main() {
+void init_HCSR04() {
+    DDRF &= ~(SWITCH_IN);
+    PORTF |= SWITCH_IN;
+
+    DDRB |= HCSR04_TRIG;
+    DDRB &= ~(HCSR04_ECHO);
+
+    PORTB |= HCSR04_ECHO | HCSR04_TRIG;
+}
+
+void setup() {
     init_74HC595();
     init_3461BS();
+    init_HCSR04();
+}
+
+uint16_t measure_distance() {
+    float duration, distance = 0;
+
+    PORTB &= ~HCSR04_TRIG;
+    _delay_us(2);
+    PORTB |= HCSR04_TRIG;
+    _delay_us(10);
+    PORTB &= ~HCSR04_TRIG;
+
+    while (!(PINB & HCSR04_ECHO));
+
+    TCNT1 = 0;
+    TCCR1B = (1 << CS11);
+
+    while (PINB & HCSR04_ECHO) ;
+
+
+    uint16_t ticks = TCNT1;
+    TCCR1B = 0;
+
+    distance = (((float)ticks / 2) * 0.0343) /2;
+
+    return distance;
+}
+
+int main() {
+    setup();
+
+    uint16_t measure = 0;
 
     while (1) {
-        for (int i = 0; i < 255; i++) {
-            for(int j = 0; j < 100; j++) {
-                PORTF |= SEG_DIG1;
-                send_serial_byte(digit_segments[i / 1000]);
-                _delay_ms(2);
-                PORTF &= ~SEG_DIG1;
-
-                PORTF |= SEG_DIG2;
-                send_serial_byte(digit_segments[(i / 100) % 10]);
-                _delay_ms(2);
-                PORTF &= ~SEG_DIG2;
-
-                PORTF |= SEG_DIG3;
-                send_serial_byte(digit_segments[(i / 10) % 10]);
-                _delay_ms(2);
-                PORTF &= ~SEG_DIG3;
-
-                PORTF |= SEG_DIG4;
-                send_serial_byte(digit_segments[i % 10]);
-                _delay_ms(2);
-                PORTF &= ~SEG_DIG4;
-            }
+        if (!(PINF & SWITCH_IN)) {
+            measure = measure_distance();
         }
+        send_serial_byte(digit_segments[measure / 1000]);
+        PORTF |= SEG_DIG1;
+        _delay_ms(2);
+        PORTF &= ~SEG_DIG1;
+
+        send_serial_byte(digit_segments[(measure / 100) % 10]);
+        PORTF |= SEG_DIG2;
+        _delay_ms(2);
+        PORTF &= ~SEG_DIG2;
+
+        send_serial_byte(digit_segments[(measure / 10) % 10]);
+        PORTF |= SEG_DIG3;
+        _delay_ms(2);
+        PORTF &= ~SEG_DIG3;
+
+        send_serial_byte(digit_segments[measure % 10]);
+        PORTF |= SEG_DIG4;
+        _delay_ms(2);
+        PORTF &= ~SEG_DIG4;
     }
 }
diff --git a/main.elf b/main.elf
index 5e189f4..7d16771 100755
Binary files a/main.elf and b/main.elf differ
diff --git a/main.hex b/main.hex
index 36624b1..3302706 100644
--- a/main.hex
+++ b/main.hex
@@ -9,36 +9,74 @@
 :100080000C946B000C946B000C946B000C946B0044
 :100090000C946B000C946B000C946B000C946B0034
 :1000A0000C946B000C946B000C946B0011241FBE1D
-:1000B000CFEFDAE0DEBFCDBF11E0A0E0B1E0E4E9D0
-:1000C000F2E002C005900D92AA30B107D9F70E9464
-:1000D000AD000C9448010C94000080B3826780BB93
+:1000B000CFEFDAE0DEBFCDBF11E0A0E0B1E0EAEFC4
+:1000C000F4E002C005900D92AA30B107D9F70E9462
+:1000D000D8000C947B020C94000080B3826780BB34
 :1000E00081B38D7881BB08953E9A259A6E9A2D989A
-:1000F0007698089527E030E090E0AC01022E02C02F
-:10010000559547950A94E2F740FF25C0469A769A9E
-:100110004FEF50E762E0415050406040E1F700C0CF
-:100120000000769846984FEF50E762E0415050400B
-:100130006040E1F700C0000021503040F1F62D9AF8
-:100140008FEF90E722E0815090402040E1F700C01F
-:1001500000002D9808954698DACFCDB7DEB72897DE
-:100160000FB6F894DEBF0FBECDBF0E9474000E9490
-:100170006D000AE010E01F8218868F81988564E682
-:1001800070E00E942001FB018F819885B8010E94D8
-:1001900020019C01CB01B8010E94200144E6E42E1D
-:1001A000F12CAF0140505F4F49835A83FC01E0506E
-:1001B000FF4FEB83FC83A90140505F4F4D835E836B
-:1001C0008E9A8EEF0E947A008FE99FE00197F1F7F7
-:1001D00000C000008E988D9AE981FA8180810E948A
-:1001E0007A008FE99FE00197F1F700C000008D9839
-:1001F0008C9AEB81FC8180810E947A008FE99FE0DC
-:100200000197F1F700C000008C98899AED81FE817A
-:1002100080810E947A008FE99FE00197F1F700C08A
-:100220000000899891E0E91AF10851F6EF81F8850C
-:100230003196EF83F887EF3FF10509F09ECF9BCF12
-:1002400097FB072E16F4009407D077FD09D00E9483
-:10025000340107FC05D03EF4909581959F4F089599
-:10026000709561957F4F0895AA1BBB1B51E107C094
-:10027000AA1FBB1FA617B70710F0A61BB70B881F36
-:10028000991F5A95A9F780959095BC01CD010895C5
-:04029000F894FFCF10
-:0A029400FECF9286CCA4A08D8084DA
+:1000F0007698089520E030E090E0AC01022E02C036
+:10010000559547950A94E2F740FF0CC0469A769AB7
+:10011000769846982F5F3F4F2830310571F72D9A1A
+:100120002D9808954698F3CF87988F9A279A269806
+:1001300085B1806C85B908950E9474000E946D009D
+:100140000C9494002F988AE08A95F1F700C02F9ABA
+:1001500085E38A95F1F700002F981E9BFECF109241
+:1001600085001092840082E0809381001E99FECF6A
+:1001700060918400709185001092810080E090E091
+:100180000E94800120E030E040E05FE30E94E60151
+:1001900028E23EE74CE05DE30E94E60120E030E02B
+:1001A00040E05FE30E94E6010E945101CB01089507
+:1001B00000D000D0CDB7DEB70E949C001C821B820D
+:1001C0000AE010E07F9904C00E94A2008C839B8308
+:1001D0008C819B8168EE73E00E943D01FB01E05041
+:1001E000FF4F80810E947A008E9A8FE39FE10197F2
+:1001F000F1F700C000008E988C819B8164E670E06E
+:100200000E943D01CB01B8010E943D01FC01E0507C
+:10021000FF4F80810E947A008D9A8FE39FE10197C2
+:10022000F1F700C000008D988C819B81B8010E947D
+:100230003D0189839A83CB01B8010E943D01FC01F5
+:10024000E050FF4F80810E947A008C9A8FE39FE1FB
+:100250000197F1F700C000008C98E981FA81E05025
+:10026000FF4F80810E947A00899A8FE39FE1019776
+:10027000F1F700C000008998A5CFAA1BBB1B51E174
+:1002800007C0AA1FBB1FA617B70710F0A61BB70B06
+:10029000881F991F5A95A9F780959095BC01CD01AB
+:1002A00008950E94C50188F09F5798F0B92F9927AB
+:1002B000B751B0F0E1F0660F771F881F991F1AF051
+:1002C000BA95C9F714C0B13091F00E94DF01B1E0D6
+:1002D00008950C94DF01672F782F8827B85F39F0D5
+:1002E000B93FCCF3869577956795B395D9F73EF4EA
+:1002F00090958095709561957F4F8F4F9F4F089592
+:10030000E89409C097FB3EF49095809570956195AF
+:100310007F4F8F4F9F4F9923A9F0F92F96E9BB2765
+:100320009395F695879577956795B795F111F8CFE1
+:10033000FAF4BB0F11F460FF1BC06F5F7F4F8F4F4C
+:100340009F4F16C0882311F096E911C0772321F042
+:100350009EE8872F762F05C0662371F096E8862FDA
+:1003600070E060E02AF09A95660F771F881FDAF731
+:10037000880F9695879597F9089557FD9058440FE3
+:10038000551F59F05F3F71F04795880F97FB991FF4
+:1003900061F09F3F79F087950895121613061406B1
+:1003A000551FF2CF4695F1DF08C01616170618063E
+:1003B000991FF1CF86957105610508940895E89419
+:1003C000BB2766277727CB0197F908950E94F9018B
+:1003D0000C946A020E945C0238F00E94630220F0D2
+:1003E000952311F00C9453020C94590211240C948F
+:1003F000E0010E94BD0170F3959FC1F3950F50E09D
+:10040000551F629FF001729FBB27F00DB11D639FC6
+:10041000AA27F00DB11DAA1F649F6627B00DA11D6C
+:10042000661F829F2227B00DA11D621F739FB00D12
+:10043000A11D621F839FA00D611D221F749F332782
+:10044000A00D611D231F849F600D211D822F762F1B
+:100450006A2F11249F5750409AF0F1F088234AF0F8
+:10046000EE0FFF1FBB1F661F771F881F9150504064
+:10047000A9F79E3F510580F00C9453020C94E001C3
+:100480005F3FE4F3983ED4F3869577956795B795EB
+:10049000F795E7959F5FC1F7FE2B880F911D969505
+:1004A000879597F9089597F99F6780E870E060E075
+:1004B00008959FEF80EC089500240A9416161706FD
+:1004C00018060906089500240A9412161306140645
+:1004D00005060895092E0394000C11F4882352F0A8
+:1004E000BB0F40F4BF2B11F460FF04C06F5F7F4F60
+:0A04F0008F4F9F4F0895F894FFCF3F
+:0A04FA0081CF9286CCA4A08D8084EF
 :00000001FF