diff --git a/firmware.asm b/firmware.asm
index 8e391b1..1383963 100644
--- a/firmware.asm
+++ b/firmware.asm
@@ -13,6 +13,11 @@
 .device ATtiny24A           ; Setup device name
 .list                       ; Enable listing
 
+;
+; Registers
+.def temp_r = r16
+.def mcu_state_r = r17
+
 ;
 ; LEDS constants
 .equ LED_DIR = DDRA     
@@ -53,41 +58,7 @@
 .equ SW_FLAG_3 = 0xb0
 .equ SW_FLAG_4 = 0x70
 
-.macro init_stack_p         ; Setup stack pointer
-  ldi @0, low(@1)
-  out SPL, @0
-.endm
-
-;
-; @0 - I/O Register
-; @1 - Data to write
-.macro outi                 ; Out to i/o reg
-  push r16
-  ldi r16, @1
-  out @0, r16
-  pop r16
-.endm
-
-;
-; @0 - SRAM Address
-; @1 - Data to write
-.macro stsi                 ; Write to SRAM
-  push r16
-  ldi r16, @1
-  sts @0, r16
-  pop r16
-.endm
-
-;
-; @0 - New state
-.macro set_state
-  push r16
-  lds r16, CURRENT_STATE_ADDRESS
-
-  sts PREVIOUS_STATE_ADDRESS, r16
-  stsi CURRENT_STATE_ADDRESS, @0
-  pop r16
-.endm
+.include "macros.asm"       ; Include macros
 
 .dseg                       ; Data segment
 .org	SRAM_START
@@ -102,7 +73,7 @@ PREVIOUS_STATE_ADDRESS: .byte 0x01
 SW_FLAGS_ADDRESS: .byte 0x01
 
 .cseg                       ; Code segment
-.org 0x00                   ; Start program at 0x00
+.org 0x00
 
 ;
 ; Setup vectors
@@ -144,18 +115,17 @@ reti
 ;
 ; Program start at reset
 start:
-  init_stack_p r16, RAMEND  ; Init stack pointer of MCU
+  init_stack_p temp_r, RAMEND  ; Init stack pointer of MCU
   set_state INIT_STATE      ; Turn MCU state to initialization
 loop:                       ; Program loop
-  lds r16, CURRENT_STATE_ADDRESS
+  lds mcu_state_r, CURRENT_STATE_ADDRESS
   init:                     ; Init state
-    cpi r16, INIT_STATE
+    cpi mcu_state_r, INIT_STATE
     brne showing
     rcall MCU_Init
-    ; rcall delay_1s
     set_state SHOWING_STATE
   showing:                  ; Showing state
-    cpi r16, SHOWING_STATE
+    cpi mcu_state_r, SHOWING_STATE
     brne polling
     
     lds r18, SW_FLAGS_ADDRESS
@@ -164,6 +134,7 @@ loop:                       ; Program loop
       brne led_off_1
       led_on_1:
         sbi LED_PORT, 0
+        outi OCR0A, 8
         rjmp led_2
       led_off_1:
         cbi LED_PORT, 0
@@ -172,6 +143,7 @@ loop:                       ; Program loop
       brne led_off_2
       led_on_2:
         sbi LED_PORT, 1
+        outi OCR0A, 10
         rjmp led_3
       led_off_2:
         cbi LED_PORT, 1
@@ -180,6 +152,7 @@ loop:                       ; Program loop
       brne led_off_3
       led_on_3:
         sbi LED_PORT, 2
+        outi OCR0A, 12
         rjmp led_4
       led_off_3:
         cbi LED_PORT, 2
@@ -188,14 +161,15 @@ loop:                       ; Program loop
       brne led_off_4
       led_on_4:
         sbi LED_PORT, 3
+        outi OCR0A, 15
         rjmp polling
       led_off_4:
         cbi LED_PORT, 3
   polling:                  ; Polling state
-    cpi r16, POLLING_STATE
+    cpi mcu_state_r, POLLING_STATE
     brne completion
   completion:
-    cpi r16, COMPLETION_STATE ; Completion state
+    cpi mcu_state_r, COMPLETION_STATE ; Completion state
     brne default
   default:                  ; Do nothing
 rjmp loop
@@ -251,36 +225,48 @@ MCU_Init:
 ret
 
 delay_before_start:
-  ldi r17, 6
+  ldi temp_r, 6
   _init_loop_loading:
     rcall effect_1
-    dec r17
+    dec temp_r
     brne _init_loop_loading
   rcall delay_1s
+  clr temp_r
 ret
 
 init_interrupts:
   ;
   ; Enable Port Change Interrupt
-  ldi r16, (1<<PCIE0)
-  out GIMSK, r16
+  ldi temp_r, (1<<PCIE0)
+  out GIMSK, temp_r
 
   ;
   ; Set Pin Change Mask Register
-  ldi r16, (1<<PCINT4) | (1<<PCINT5)| (1<<PCINT6) | (1<<PCINT7)
-  out PCMSK0, r16
+  ldi temp_r, (1<<PCINT4) | (1<<PCINT5)| (1<<PCINT6) | (1<<PCINT7)
+  out PCMSK0, temp_r
 
-  clr r16
+  ;
+  ; Setup timer
+  ldi temp_r, (1<<WGM01) | (1<<COM0A0)        ; CTC Mode
+  out TCCR0A, temp_r
+
+  ldi temp_r, 4
+  out OCR0A, temp_r
+
+  ldi temp_r, (1<<CS00) | (1<<CS01)           ; Prescale
+  out TCCR0B, temp_r
+
+  clr temp_r
 
   sei                       ; Enable Global Interrupts
 ret
 
 init_ports:                 ; Init MCU ports
   ; Setup PORTA 
-  ldi r16, 0x0f
-  out DDRA, r16             ; Set directions of leds and buttons
-  swap r16
-  out PORTA, r16            ; Set low signal on leds and pull-up on buttons
+  ldi temp_r, 0x0f
+  out DDRA, temp_r             ; Set directions of leds and buttons
+  swap temp_r
+  out PORTA, temp_r            ; Set low signal on leds and pull-up on buttons
 
   ; Setup PORTB
   sbi DDRB, BUZZ_PIN        ; Set direction of buzzer pin to output
diff --git a/firmware.hex b/firmware.hex
index ba9a2b8..5e30c66 100644
--- a/firmware.hex
+++ b/firmware.hex
@@ -1,28 +1,29 @@
 :020000020000FC
 :100000001CC018950EC01895189518951895189538
 :100010001895189518951895189518951895189578
-:1000200018951F932F932FB719B3107F7FD010937C
-:1000300062002FBF2F911F9118950FED0DBF0F93E9
-:1000400000916000009361000F9301E00093600055
-:100050000F910F9100916000013061F44FD00F9328
-:1000600000916000009361000F9302E00093600034
-:100070000F910F910230B1F420916200203E11F4F3
-:10008000D89A01C0D898203D11F4D99A01C0D998C6
-:10009000203B11F4DA9A01C0DA98203711F4DB9A88
-:1000A00001C0DB98033001F4043001F4D3CF1F9377
-:1000B0002F933F934F934BB30F9301EF0BBB0F91D4
-:1000C00035D011E033E020EF110F210F2BBB2ED0E4
-:1000D0003A95C9F70F9308EF0BBB0F9127D018E0A3
-:1000E00033E020EF1695210F2BBB20D03A95C9F7AE
-:1000F0004BBB4F913F912F911F91089510D008D085
-:1001000001D0089516E0D3DF1A95E9F71BD00895C2
-:1001100000E10BBF00EF02BB0027789408950FE0C9
-:100120000ABB02950BBBBA9AC29808952F933F93CE
-:1001300021E43FEE3A95F1F72A95E1F700003F916F
-:100140002F9108958F939F9324E683EC99E0019774
-:10015000F1F72A95D1F724E69F918F9108954D6587
-:100160006D6F7279206C65642067616D652E205714
-:1001700072697474656E206279205365726765795F
-:10018000205961726B6F762032372E30392E323023
-:0201900032310A
+:1000200018951F932F932FB719B3107F8ED010936D
+:1000300062002FBF2F911F9118950FED0DBF1093E8
+:1000400061000F9301E0009360000F911091600038
+:10005000113041F45BD0109361000F9302E00093E4
+:1000600060000F91123031F520916200203E31F492
+:10007000D89A0F9308E006BF0F9101C0D898203D91
+:1000800031F4D99A0F930AE006BF0F9101C0D998B5
+:10009000203B31F4DA9A0F930CE006BF0F9101C0B8
+:1000A000DA98203731F4DB9A0F930FE006BF0F91F7
+:1000B00001C0DB98133001F4143001F4C7CF1F9353
+:1000C0002F933F934F934BB30F9301EF0BBB0F91C4
+:1000D0003CD011E033E020EF110F210F2BBB35D0C6
+:1000E0003A95C9F70F9308EF0BBB0F912ED018E08C
+:1000F00033E020EF1695210F2BBB27D03A95C9F797
+:100100004BBB4F913F912F911F91089517D009D06C
+:1001100001D0089506E0D3DF0A95E9F722D0002741
+:10012000089500E10BBF00EF02BB02E400BF04E052
+:1001300006BF03E003BF0027789408950FE00ABBD1
+:1001400002950BBBBA9AC29808952F933F9321E46E
+:100150003FEE3A95F1F72A95E1F700003F912F9194
+:1001600008958F939F9324E683EC99E00197F1F72C
+:100170002A95D1F724E69F918F9108954D656D6F73
+:100180007279206C65642067616D652E20577269F5
+:100190007474656E206279205365726765792059A1
+:1001A00061726B6F762032372E30392E3230323119
 :00000001FF
diff --git a/macros.asm b/macros.asm
new file mode 100644
index 0000000..4dea419
--- /dev/null
+++ b/macros.asm
@@ -0,0 +1,41 @@
+;
+; Initialize stack pointer
+; @0 - Temp register
+.macro init_stack_p
+  ldi @0, low(@1)
+  out SPL, @0
+  .ifdef SPH 
+    ldi @0, high(RAMEND) 
+    out SPH, @0 
+  .endif
+.endm
+
+;
+; out immediate
+; @0 - I/O Register
+; @1 - Data to write
+.macro outi
+  push r16
+  ldi r16, @1
+  out @0, r16
+  pop r16
+.endm
+
+;
+; sts immediate
+; @0 - SRAM Address
+; @1 - Data to write
+.macro stsi
+  push r16
+  ldi r16, @1
+  sts @0, r16
+  pop r16
+.endm
+
+;
+; Set MCU global state
+; @0 - New state
+.macro set_state
+  sts PREVIOUS_STATE_ADDRESS, mcu_state_r         ; Write previous state to SRAM
+  stsi CURRENT_STATE_ADDRESS, @0
+.endm
\ No newline at end of file