diff --git a/i2c.c b/i2c.c
index c4efba6..c455c1e 100644
--- a/i2c.c
+++ b/i2c.c
@@ -1,8 +1,11 @@
-#include <avr/io.h>
-#include <util/delay.h>
 #include "i2c.h"
 
-void initI2C() {
+#include <avr/io.h>
+#include <util/delay.h>
+#include <util/twi.h>
+#include <stdint.h>
+
+void I2C_init() {
     // Set the prescaler to 1
     TWSR &= ~(1 << TWPS0);
     TWSR &= ~(1 << TWPS1);
@@ -10,35 +13,58 @@ void initI2C() {
     TWBR = ((F_CPU / 100000) - 16) / 2;
 }
 
-void I2C_start() {
+uint8_t I2C_start(uint8_t addr) {
     // Send the start condition
     TWCR = (1 << TWINT) | (1 << TWSTA) | (1 << TWEN);
+
     // Wait for the start condition to be sent
     while (!(TWCR & (1 << TWINT)))
         ;
+
+    // Load the address of the slave device
+    TWDR = addr;
+
+    // Clear the TWINT bit to start the transmission
+    TWCR = (1 << TWINT) | (1 << TWEN);
+
+    // Wait for the address to be sent
+    while (!(TWCR & (1 << TWINT)))
+        ;
+
+    // Get the status of the transmission
+    uint8_t status = TWSR & 0xF8;
+
+    // Return true if the slave acknowledged the address
+    return (status == TW_MT_SLA_ACK || status == TW_MR_SLA_ACK);
 }
 
 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));
+    while (TWCR & (1 << TWSTO))
+        ;
 }
 
-void I2C_write(uint8_t data) {
+uint8_t 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)));
+    while (!(TWCR & (1 << TWINT)))
+        ;
+
+    // Return true if the data was sent
+    return (TWSR & 0xF8) == TW_MT_DATA_ACK;
 }
 
 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)));
+    while (!(TWCR & (1 << TWINT)))
+        ;
     // Return received data
     return TWDR;
 }
\ No newline at end of file
diff --git a/i2c.h b/i2c.h
index 47fa6fe..5749df3 100644
--- a/i2c.h
+++ b/i2c.h
@@ -1,7 +1,8 @@
 #pragma once
+#include <stdint.h>
 
-void initI2C();
-void I2C_start();
+void I2C_init();
+uint8_t I2C_start(uint8_t addr);
 void I2C_stop();
-void I2C_write(uint8_t data);
+uint8_t I2C_write(uint8_t data);
 uint8_t I2C_read(uint8_t ack);
\ No newline at end of file
diff --git a/main.c b/main.c
index 1808f57..747a162 100644
--- a/main.c
+++ b/main.c
@@ -5,42 +5,16 @@
  * need pull-up resistors for the I2C lines.
  */
 
+#define LED_PIN PB5  // Define the pin connected to the LED
+
 #include <avr/io.h>
 #include <util/delay.h>
 #include <math.h>
 
-#define LED_PIN PB5  // Define the pin connected to the LED
-#define TEMP_SENSOR_ADDR 0x48
-#define TEMP_REG_ADDR 0x00
-
 #include "MPU6050.h"
 #include "i2c.h"
 #include "uart.h"
 
-float readTemperature() {
-    uint16_t temperature;
-
-    // Send start condition
-    I2C_start();
-    // Send device address with write operation
-    I2C_write((TEMP_SENSOR_ADDR << 1) | 0);
-    // Send temperature register address
-    I2C_write(0x00);
-    // Repeat start
-    I2C_start();
-    // Send device address with read operation
-    I2C_write((TEMP_SENSOR_ADDR << 1) | 1);
-    // Read temperature data MSB
-    temperature = (uint16_t)(I2C_read(1)) << 8;
-    // Read temperature data LSB
-    temperature |= I2C_read(0);
-    // Send stop condition
-    I2C_stop();
-
-    // Convert raw data to temperature in Celsius
-    return (float)temperature * 0.0625;
-}
-
 void blink() {
     // Set the LED pin as output
     DDRB |= (1 << LED_PIN);
@@ -59,7 +33,6 @@ void blink() {
 }
 
 int main(void) {
-    // initI2C();
     initUART();
 
     while(1) {
@@ -67,18 +40,5 @@ int main(void) {
         _delay_ms(1000);
     }
 
-
-    // float temperature;
-
-    // while (1) {
-    //     temperature = readTemperature();
-    //     // Print temperature over UART
-    //     UART_println("Temperature: ");
-    //     UART_transmit((uint8_t)(temperature / 10.0) + '0');
-    //     UART_transmit((uint8_t)fmod(temperature, 10.0) + '0');
-    //     UART_println(" °C");
-    //     _delay_ms(1000);  // Delay for 1 second
-    // }
-
-    // return 0;
+    return 0;
 }