diff --git a/Makefile b/Makefile
index 246130f..cfb8c56 100644
--- a/Makefile
+++ b/Makefile
@@ -17,7 +17,7 @@ $(TARGET): $(SRC)
 	rm $@.elf
 
 flash:
-	avrdude -p m32u4 -P /dev/ttyACM0 -c avr109 -Uflash:w:"$(TARGET).hex":i
+	avrdude -p m32u4 -P /dev/ttyACM0 -c stk500v2 -Uflash:w:"$(TARGET).hex":i
 
 clean:
 	rm -fr ./src/*.o ./build/
diff --git a/docs/Pinout-NANO_latest.pdf b/docs/Pinout-NANO_latest.pdf
deleted file mode 100644
index 3228b35..0000000
Binary files a/docs/Pinout-NANO_latest.pdf and /dev/null differ
diff --git a/src/inverter.c b/src/inverter.c
index 5f813fe..a870b67 100644
--- a/src/inverter.c
+++ b/src/inverter.c
@@ -1,110 +1,46 @@
 #include <avr/io.h>
 #include <util/delay.h>
 #include <avr/interrupt.h>
-#include <stdio.h>
 
-void togglePin(uint8_t port, uint8_t pin);
+#include "include/uart.h"
+#include "include/timer.h"
 
-void initCtcTimer0(void);
-void initOverflowTimer0(void);
-void initOverflowTimer1(void);
-
-void initUart(unsigned int baud);
-void uart_sendByte(uint8_t byte);
-int uart_printf(char var, FILE *stream);
-
-static FILE stream = FDEV_SETUP_STREAM(uart_printf, NULL, _FDEV_SETUP_WRITE);
 
 uint8_t sec = 0;
+uint16_t cnt = 0;
 
 int main()
 {
-	stdout = &stream;
+	/* With this MCU frequency, it is 38,4k baud */
+	uart_init(12);
 
-	/* Make PB1 & PB2 an output pin */
-	DDRB |= (1 << DDB1);
+	/* Make PB2 an output pin */
 	DDRB |= (1 << DDB2);
 
-	/* With this frequency, it is 38,4k baud */
-	initUart(12);
+	initCtcTimer0();
+
+	sei();
 
 	while (1) {
-		_delay_ms(1000);
-		printf("Seconds passed: %u\n\r", sec++);
+
+		if ((PORTB & (1 << PB2)) > 1)
+		{
+			_delay_ms(1);
+			PORTB &= ~(1 << PB2);
+		}
+
+		if (cnt == 65500) {
+			printf("%u\n\r", TCNT0);
+			cnt = 0;
+		}	
+
+		cnt++;
 	}
 
 	return 0;
 }
 
-
-
-void initCtcTimer0(void)
-{
-	/* Initialize counter 0 */
-	TCNT0 = 0;
-
-	/* Enable Counter0 Compare Match A Interrupt */
-	TIMSK0 |= (1 << OCIE0A);
-	
-	/* Select clock. Prescaler of 0*/
-	TCCR0B |= (1 << CS00);
-	
-	/* Use CTC Mode */
-	TCCR0A |= (1 << WGM01);
-
-	/* 
-	 * OCR0A contains TOP value for counter:
-	 */
-	OCR0A = 200;
-}
-
-void initOverflowTimer0(void)
-{
-	/* Prescaler = 0 */
-	TCCR0B |= (1<<CS00);
-
-	/* Enable Timer Overflow Interrupt */
-	TIMSK0 |= (1<<TOIE0);
-}
-
-void initOverflowTimer1(void)
-{
-	TCCR1B |= (1<<CS11);
-	TIMSK1 |= (1<<TOIE1);
-}
-
 ISR(TIMER0_COMPA_vect)
 {
-	PORTB ^= (1 << PB2);
-}
-
-void initUart(unsigned int baud)
-{
-	/* Set baud rate */
-	UBRR1H = (unsigned char)(baud>>8);
-	UBRR1L = (unsigned char)baud;
-	
-	/* Enable receiver and transmitter */
-	UCSR1B = (1<<RXEN1)|(1<<TXEN1);
-
-	/* Set frame format */
-	UCSR1C |= (3<<UCSZ10); // 8 data bits
-	// UCSR1C &= ~(1<<USBS1); // 1 stop bit
-}
-
-void uart_sendByte(uint8_t byte)
-{
-	/* Wait for empty transmit buffer */
-	while (!(UCSR1A & (1<<UDRE1)))
-	{
-	}
-
-	/* Put data into buffer, sends the data */
-	UDR1 = byte;
-}
-
-int uart_printf(char var, FILE *stream)
-{
-	uart_sendByte(var);
-	return 0;
-}
+	PORTB |= (1 << PB2);
+}
\ No newline at end of file