diff --git a/main.c b/main.c
index 1808f57..07501f4 100644
--- a/main.c
+++ b/main.c
@@ -12,10 +12,73 @@
 #define LED_PIN PB5  // Define the pin connected to the LED
 #define TEMP_SENSOR_ADDR 0x48
 #define TEMP_REG_ADDR 0x00
+#define BAUD 9600  // 9600 seems to be the highest the ATmega328P can handle in this config
 
-#include "MPU6050.h"
-#include "i2c.h"
-#include "uart.h"
+void initI2C() {
+    // Set the prescaler to 1
+    TWSR &= ~(1 << TWPS0);
+    TWSR &= ~(1 << TWPS1);
+    // Set the bit rate to 100kHz
+    TWBR = ((F_CPU / 100000) - 16) / 2;
+}
+
+void I2C_start() {
+    // Send the start condition
+    TWCR = (1 << TWINT) | (1 << TWSTA) | (1 << TWEN);
+    // Wait for the start condition to be sent
+    while (!(TWCR & (1 << TWINT)))
+        ;
+}
+
+void I2C_stop() {
+    // Send the stop condition
+    TWCR = (1 << TWINT) | (1 << TWSTO) | (1 << TWEN);
+    // Wait for the stop condition to be sent
+    while (TWCR & (1 << TWSTO));
+}
+
+void I2C_write(uint8_t data) {
+    // Load the data into the data register
+    TWDR = data;
+    // Start transmission of data
+    TWCR = (1 << TWINT) | (1 << TWEN);
+    // Wait for the data to be sent
+    while (!(TWCR & (1 << TWINT)));
+}
+
+uint8_t I2C_read(uint8_t ack) {
+    // Enable TWI, generate ACK (if ack = 1) and clear TWI interrupt flag
+    TWCR = (1 << TWINT) | (1 << TWEN) | (ack << TWEA);
+    // Wait until TWI finish its current job (read operation)
+    while (!(TWCR & (1 << TWINT)));
+    // Return received data
+    return TWDR;
+}
+
+void initUART() {
+    // Set baud rate
+    UBRR0H = (uint8_t)(F_CPU / (BAUD * 16UL) - 1) >> 8;
+    UBRR0L = (uint8_t)(F_CPU / (BAUD * 16UL) - 1);
+    // Enable receiver and transmitter
+    UCSR0B |= (1 << RXEN0) | (1 << TXEN0);
+    // Set frame format: 8 data, 1 stop bit
+    UCSR0C |= (1 << UCSZ01) | (1 << UCSZ00);
+}
+
+void UART_transmit(uint8_t data) {
+    // Wait for empty transmit buffer
+    while (!(UCSR0A & (1 << UDRE0)));
+    // Put data into buffer, sends the data
+    UDR0 = data;
+}
+
+void UART_println(const char *str) {
+    // Transmit each character until NULL character is encountered
+    while (*str) UART_transmit(*str++);
+    // Transmit carriage return and line feed characters
+    UART_transmit('\r');
+    UART_transmit('\n');
+}
 
 float readTemperature() {
     uint16_t temperature;
diff --git a/makefile b/makefile
index efd39fb..3e28edb 100644
--- a/makefile
+++ b/makefile
@@ -14,30 +14,27 @@ CFLAGS = -std=c2x -Wall -Wno-array-bounds -mmcu=$(MCU) -DF_CPU=16000000UL -O3
 # Source files
 SRCS = $(wildcard *.c)
 
-# Object files directory
-OBJDIR = build
-
 # Object files
-OBJS = $(addprefix $(OBJDIR)/,$(SRCS:.c=.o))
+OBJS = $(SRCS:.c=.o)
 
 # Target file
 TARGET = main
 
 # Default target
-all: $(OBJDIR) $(TARGET).hex
+all: $(TARGET).hex
 
 # Compile C files
-$(OBJDIR)/%.o: %.c | $(OBJDIR)
+%.o: %.c
 	$(CC) $(CFLAGS) -c $< -o $@
 
 # Link object files
-$(OBJDIR)/$(TARGET).elf: $(OBJS)
-	$(CC) $(CFLAGS) $(OBJS) -o $(OBJDIR)/$(TARGET).elf
-	avr-strip $(OBJDIR)/$(TARGET).elf
+$(TARGET).elf: $(OBJS)
+	$(CC) $(CFLAGS) $(OBJS) -o $(TARGET).elf
+	avr-strip $(TARGET).elf
 
 # Convert ELF to HEX
-$(TARGET).hex: $(OBJDIR)/$(TARGET).elf
-	avr-objcopy -O ihex -R .eeprom $(OBJDIR)/$(TARGET).elf $(TARGET).hex
+$(TARGET).hex: $(TARGET).elf
+	avr-objcopy -O ihex -R .eeprom $(TARGET).elf $(TARGET).hex
 
 # Flash the program
 flash: $(TARGET).hex
@@ -45,22 +42,18 @@ flash: $(TARGET).hex
 
 # Run the program in QEMU
 qemu: $(TARGET).elf
-	qemu-system-avr -machine $(QEMU_MACHINE_NAME) -bios $(OBJDIR)/$(TARGET).elf
+	qemu-system-avr -machine $(QEMU_MACHINE_NAME) -bios $(TARGET).elf
 
 # View the generated assembly
 asm: $(TARGET).hex
-	avr-objdump -S $(OBJDIR)/$(TARGET).elf
+	avr-objdump -S $(TARGET).elf
 
 serial:
 	picocom -b 9600 /dev/ttyUSB0
 
-# Create build directory
-$(OBJDIR):
-	mkdir -p $(OBJDIR)
-
 # Clean
 clean:
-	rm -rf $(OBJDIR) $(TARGET).hex
+	rm -f $(OBJS) $(TARGET).elf $(TARGET).hex
 
-size: $(TARGET).hex
-	avr-size --mcu=atmega328p $(TARGET).hex
+size: main.hex
+	avr-size --mcu=atmega328p main.hex
\ No newline at end of file